Road curvature estimation system

ABSTRACT

A processor using a first Kalman filter estimates a host vehicle state from speed and yaw rate, the latter of which may be from a yaw rate sensor if speed is greater than a threshold, and, if less, from a steer angle sensor and speed. Road curvature parameters are estimated from a curve fit of a host vehicle trajectory or from a second Kalman filter for which a state variable may be responsive to a plurality of host state variables. Kalman filters may incorporate adaptive sliding windows. Curvature of a most likely road type is estimated with an interacting multiple model (IMM) algorithm using models of different road types. A road curvature fusion subsystem provides for fusing road curvature estimates from a plurality of curvature estimators using either host vehicle state, a map database responsive to vehicle location, or measurements of a target vehicle with a radar system.

CROSS-REFERENCE TO RELATED APPLICATIONS

The instant application is a continuation-in-part of U.S. applicationSer. No. 10/620,749 filed on 15 Jul. 2003, which claims the benefit ofprior U.S. Provisional Application Ser. No. 60/396,211 filed on Jul. 15,2002. The instant application also claims the benefit of prior U.S.Provisional Application Ser. No. 60/532,344 filed on Dec. 24, 2003. Theabove-identified applications are incorporated herein by reference intheir entirety.

BRIEF DESCRIPTION OF THE DRAWINGS

In the accompanying drawings:

FIG. 1 illustrates a block diagram of hardware associated with apredictive collision sensing system;

FIG. 2 illustrates a coverage pattern of a radar beam used by thepredictive collision sensing system;

FIG. 3 depicts a driving scenario for purposes of illustrating theoperation of the predictive collision sensing system;

FIG. 4 illustrates a block diagram of the hardware and an associatedsignal processing algorithm of the predictive collision sensing system;

FIG. 5 illustrates a flow chart of an associated signal processingalgorithm of the predictive collision sensing system;

FIG. 6 illustrates a geometry used for determining curvature parametersof a roadway;

FIG. 7 illustrates the geometry of an arc;

FIGS. 8 a-d illustrates an example of the estimation of target position,lateral velocity, and road curvature parameters for a straight roadway;

FIGS. 9 a-b illustrate an example of the target state RMS errors fromunconstrained and constrained filtering on the straight roadway,corresponding to FIGS. 8 a-d;

FIGS. 10 a-d illustrate an example of the estimation of target position,lateral velocity, and road curvature parameters for a curved roadway;

FIGS. 11 a-b illustrate an example of the target state RMS errors fromunconstrained and constrained filtering for the curved roadway,corresponding to FIGS. 10 a-d;

FIGS. 12 a-d illustrate an example of the estimation of target position,lateral velocity, and associated RMS errors for a straight roadwayinvolving a lane change;

FIGS. 13 a-d illustrates an example of the estimation of targetposition, lateral velocity, and their RMS errors for a curved roadwayinvolving a lane change;

FIG. 14 illustrates a block diagram of hardware associated with anotherembodiment of a predictive collision sensing system;

FIG. 15 illustrates a free-body diagram of a steered wheel;

FIG. 16 a illustrates a geometry of a bicycle model of a vehicleundergoing a turn;

FIG. 16 b illustrates a geometry of the steered wheel illustrated inFIG. 16 a.;

FIG. 17 illustrates a switching curve;

FIG. 18 illustrates a flow chart of a process associated with theswitching curve illustrated in FIG. 17;

FIG. 19 illustrates a block diagram of a road curvature estimationsubsystem for estimating road curvature from host vehicle stateestimates;

FIG. 20 illustrates a curvature filter associated with a firstembodiment of a curvature estimator;

FIG. 21 illustrates a curvature filter associated with a fourthembodiment of a curvature estimator;

FIG. 22 illustrates various types of roads and associated road models;

FIG. 23 illustrates a block diagram of a tenth embodiment of a roadcurvature estimation subsystem;

FIG. 24 illustrates a flow chart of an interacting multiple modelalgorithm;

FIG. 25 illustrates a block diagram of a curvature estimation subsystemresponsive to vehicle location and associated road curvature data froman associated map system;

FIG. 26 illustrates a block diagram of a curvature estimation subsystemresponsive to radar measurements of a target vehicle on the roadway; and

FIG. 27 illustrates a block diagram of a predictive collision sensingsystem comprising a plurality of road curvature estimation subsystemsand an associated road curvature fusion subsystem.

DESCRIPTION OF EMBODIMENT(S)

Referring to FIG. 1, a predictive collision sensing system 10incorporated in a host vehicle 12, comprises a radar system 14 forsensing objects external to the host vehicle 12, and a set of sensors,including a yaw rate sensor 16, e.g. a gyroscopic sensor, and a speedsensor 18, for sensing motion of the host vehicle 12. The yaw ratesensor 16 and speed sensor 18 respectively provide measurements of theyaw rate and speed of the host vehicle 12. The radar system 14, e.g. aDoppler radar system, comprises an antenna 20 and a radar processor 22,wherein the radar processor 22 generates the RF signal which istransmitted by the antenna 20 and which is reflected by objects in viewthereof. The radar processor 22 demodulates the associated reflected RFsignal that is received by the antenna 20, and detects a signal that isresponsive to one or more objects that are irradiated by the RF signaltransmitted by the antenna 20. For example, the radar system 14 providestarget range, range rate and azimuth angle measurements in host vehicle12 fixed coordinates. Referring to FIG. 2, the antenna 20 is adapted togenerate a radar beam 23 of RF energy that is, for example, eitherelectronically or mechanically scanned across an azimuth range, e.g.+/−γ, e.g. +/−50 degrees, responsive to a beam control element 24, andwhich has a distance range, e.g. about 100 meters, from the host vehicle12 that is sufficiently far to enable a target to be detectedsufficiently far in advance of a prospective collision with the hostvehicle 12 so as to enable a potentially mitigating action to be takenby the host vehicle 12 so as to either avoid the prospective collisionor mitigate damage or injury as a result thereof. The radar processor22, yaw rate sensor 16, and speed sensor 18 are operatively connected toa signal processor 26 that operates in accordance with an associatedpredictive collision sensing algorithm to determine whether or not acollision with an object, e.g. a target vehicle 36 (illustrated in FIG.3), is likely, and if so, to also determine an action to be takenresponsive thereto, for example, one or more of activating an associatedwarning system 28 or safety system 30 (e.g. frontal air bag system), orusing a vehicle control system 32 (e.g. an associated braking orsteering system) to take evasive action so as to either avoid theprospective collision or to reduce the consequences thereof.

Referring to FIG. 3, the host vehicle 12 is shown moving along amultiple lane roadway 34, either straight or curved, and there is alsoshown a target vehicle 36 moving in an opposite direction, towards thehost vehicle 12. Generally, there can be any number of target vehicles36 that can fit on the roadway 34, each moving in the same or oppositedirection as the host vehicle 12. These target vehicles 36 can either bein the host lane 38 or in a neighboring lane 40 either adjacent to orseparated from the host lane 38, but generally parallel thereto. Forpurposes of analysis, it is assumed that the host vehicle 12 moves alongthe center line 41 of its lane 38 steadily without in-lane wandering,and the road curvatures of all the parallel lanes 38, 40 are the same.Road curvature is assumed small such that the differences between theheading angles of the host vehicle 12 and any detectable target vehicles36 are smaller than 15 degrees.

Referring to FIG. 4, the predictive collision sensing system 10 uses themeasurements of speed U^(h) and yaw rate ω^(h) of the host vehicle 12from the speed sensor 18 and the yaw rate sensor 16 respectivelytherein; and the measurements of target range r, range rate {dot over(r)} and azimuth angle η for all target vehicles 36 from the radarsystem 14 mounted on the host vehicle 12; along with the correspondingerror covariance matrices of all these measurements, to estimate eachtarget's two dimensional position, velocity and acceleration [x, {dotover (x)}, {umlaut over (x)}, y, {dot over (y)}, ÿ]^(r) in the hostfixed coordinate system at every sampling instance, preferably with anerror as small as possible. The predictive collision sensing system 10comprises 1) a road curvature estimation subsystem 42 for estimating thecurvature of the roadway 34 using measurements from the host vehiclemotion sensors, i.e. the yaw rate sensor 16 and speed sensor 18; 2) anunconstrained target state estimation subsystem 44 for estimating thestate of a target illuminated by the radar beam 23 and detected by theradar processor 22; 3) a constrained target state estimation subsystem46 for estimating the state of the constraint on the target, assumingthat the target is constrained to be on the roadway 34, either in thehost lane 38 or in a neighboring lane 40, for each possible lane 38, 40;4) a target state decision subsystem 48 for determining whether the bestestimate of the target state is either the unconstrained target state,or a target state constrained by one of the constraints; and 5) a targetstate fusion subsystem 50 for fusing the unconstrained target stateestimate with the appropriate constraint identified by the target statedecision subsystem 48 so as to generate a fused target state. The bestestimate of target state—either the unconstrained target state or thefused target state—is then used by a decision or control subsystem fordetermining whether or not the host vehicle 12 is at risk of collisionwith the target, and if so, for determining and effecting what the bestcourse of action is to mitigate the consequences thereof, e.g. by actionof either the warning system 28, the safety system 30, or the vehiclecontrol system 32, or some combination thereof. When possible, the useof the geometric structure of the roadway 34 as a constraint to thetarget kinematics provides for a more accurate estimate of the targetstate, which thereby improves the reliability of any actions takenresponsive thereto.

Referring also to FIG. 5, illustrating a method 500 of detecting thestate, i.e. kinematic state variables, of a target in view of the hostvehicle 12, the steps of which are, for example, carried out by thesignal processor 26, in steps (502) and (504), the speed U^(h) and yawrate ω^(h) of the host vehicle 12 relative to the roadway 34 arerespectively read from the speed sensor 18 and the yaw rate sensor 16respectively. Then, in step (506), the curvature parameters andassociated covariance thereof of the roadway 34 are estimated usingfirst 52 and second 54 Kalman filters that respectively estimate thestate (i.e. kinematic state variables of the host vehicle 12) andassociated covariance thereof of the host vehicle 12, and then thecurvature parameters and associated covariance thereof of the roadway34, as described hereinbelow, wherein the curvature parameters andassociated covariance thereof of the roadway 34 are then subsequentlyused by the constrained target state estimation subsystem 46 to generateassociated constraints on the possible location of a prospective targetvehicle 36.

A well-designed and constructed roadway 34 can be described by a set ofparameters, including curvature, wherein the curvature of a segment ofthe roadway 34 is defined as: $\begin{matrix}{C = \frac{1}{R}} & (1)\end{matrix}$where R is the radius of the segment. In general, for a piece of smoothroadway 34, the curvature variation can be described as a function of adistance l along the roadway 34 by a so-called clothoid model, i.e.:$\begin{matrix}{C = {{C_{0} + {\frac{\mathbb{d}C}{\mathbb{d}l}l}} = {C_{0} + {C_{1}l}}}} & (2)\end{matrix}$where C₁=1/A² and A is referred to as the clothoid parameter.

Referring to FIG. 6, the heading angle θ defining the heading directionis given by: $\begin{matrix}{\theta = {\theta_{0} + {\int_{0}^{1}{{C(\tau)}\quad{{\mathbb{d}\tau}.}}}}} & (3)\end{matrix}$

Substituting equation (2) into equation (3) givesΔθ=θ−θ₀ =C ₀ l+C ₁ l ²/2  (4)

Referring to FIG. 6, the equation of the roadway 34, i.e. the roadequation, in x-y coordinates is given by: $\begin{matrix}{x = {x_{0} + {\int_{0}^{1}{\cos\quad{\theta(\tau)}{\mathbb{d}\tau}\quad{and}}}}} & (5) \\{y = {y_{0} + {\int_{0}^{1}{\sin\quad{\theta(\tau)}\quad{{\mathbb{d}\tau}.}}}}} & (6)\end{matrix}$

Assuming the heading angle θ to be within 15 degrees, i.e. |θ<15°,equations (5) and (6) can be approximated by:Δx=x−x ₀ ≈l  (7) $\begin{matrix}{{\Delta\quad y} = {{y - y_{0}} \approx {{C_{0}{l^{2}/2}} + {C_{1}{l^{3}/6}}} \approx {{C_{0}\frac{\Delta\quad x^{2}}{2}} + {C_{1}\frac{\Delta\quad x^{3}}{6}}}}} & (8)\end{matrix}$

Accordingly, the roadway 34 is modeled by an incremental road equationin terms of curvature coefficients (or parameters): C₀ and C₁. Thisincremental road equation describes a broad range of road shapes asfollows: 1) Straight roadway 34: C₀=0 and C₁=0; 2) circular roadway 34:C₁=0; and 3) a general roadway 34 with an arbitrary shape for which thechange in heading angle θ is less than 15 degrees: C₀>0.

The road curvature parameters C₀ and C₁ are estimated using data frommotion sensors (yaw rate sensor 16 and speed sensor 18) in the hostvehicle 12, based upon the assumption that the host vehicle 12 movesalong the center line 41 of the roadway 34 or associated host lane 38.

The road curvature parameters C₀ and C₁ can be calculated from data ofω, {dot over (ω)}, U, {dot over (U)} responsive to measurements of yawrate ω^(h) and speed U^(h) of the host vehicle 12 from the availablehost vehicle 12 motion sensors. However, generally the measurements ofyaw rate ω^(h) and speed U^(h), from the yaw rate sensor 16 and speedsensor 18 respectively, are noisy. A host state filter implemented by afirst Kalman filter 52 is beneficial to generate estimates of ω, {dotover (ω)}, U, {dot over (U)} from the associated noisy measurements ofyaw rate ω^(h) and speed U^(h); after which a curvature filterimplemented by a second Kalman filter 54 is used to generate smoothedestimates of the curvature parameters C₀ and C₁. The dynamics of thehost vehicle 12 for the host state filter follows a predefined set ofkinematic equations-(constant velocity in this case) given by:x _(k+1) ^(h) =F _(k) ^(h) ·x _(k) ^(h) +w _(k) ^(h) , w _(k) ^(h) ˜N(0,Q _(k) ^(h))  (9)z _(k) ^(h) =H _(k) ^(h) ·x _(k) ^(h) +v _(k) ^(h) , v _(k) ^(h) ˜N(0, R_(k) ^(h))  (10)where $\begin{matrix}{{{F_{k}^{h} = \begin{bmatrix}1 & T & 0 & 0 \\0 & 1 & 0 & 0 \\0 & 0 & 1 & T \\0 & 0 & 0 & 1\end{bmatrix}},{H_{k}^{h} = \begin{bmatrix}1 & 0 & 0 & 0 \\0 & 0 & 1 & 0\end{bmatrix}},{{\underset{\_}{x}}_{k}^{h} = \begin{bmatrix}U \\\overset{.}{U} \\\omega \\\overset{.}{\omega}\end{bmatrix}_{k}}}{{{and}\quad{\underset{\_}{z}}_{k}^{h}} = \begin{bmatrix}U^{h} \\\omega^{h}\end{bmatrix}_{k}}} & (11)\end{matrix}$and where T is the sampling period, superscript (•)^(h) is used toindicate that the filter is the host filter, and U^(h) and ω^(h) arehost vehicle 12 speed and yaw rate measurements. The first Kalman filter52 is implemented to estimate the host state {circumflex over (x)}_(k|k) ^(h) and its error covariance as illustrated in FIG. 4.

The estimate of the host state from the first Kalman filter 52, i.e. thehost state filter, is then used to generate a synthetic measurement thatis input to the second Kalman filter 54, i.e. curvature coefficient (orparameter) filter, wherein the associated Kalman filters 52, 54 operatein accordance with the Kalman filtering process described more fully inthe Appendix hereinbelow. The relationship between the road curvatureparameters C₀, C₁ and the host state variables ω, {dot over (ω)}, U,{dot over (U)} is derived as follows:

From equation (4), the radius R of road curvature is expressed generallyas a function R(l) of the distance l along the roadway, as isillustrated in FIG. 7. Taking the time derivative on both sides ofequation (4) yields:{dot over (θ)}=C ₀ ·i+C ₁ ·l·i=(C ₀ +C ₁ ·l)·i  (12)

Noting that {dot over (θ)}=ω, the yaw rate of the host vehicle 12, andthat i=U, the speed of the host vehicle 12, and substituting theclothoid model of equation (2) in equation (12), yields:ω=C·U  (13)or $\begin{matrix}{C = {\frac{\omega}{U}.}} & (14)\end{matrix}$

Clothoid parameter C₀ is given as the value of curvature C at l=0, or$\begin{matrix}{C_{0} = {{C❘_{l = 0}} = {\frac{\omega}{U}.}}} & (15)\end{matrix}$

Taking the derivative on both sides of equation (14) yields$\begin{matrix}{\overset{.}{C} = {\frac{\overset{.}{\omega}}{U} - {\frac{\omega \cdot \overset{.}{U}}{U^{2}}.}}} & (16)\end{matrix}$

Using the definition of C₁, from equation (2), C₁ may be expressed interms of the host state as follows: $\begin{matrix}{C_{1} = {\frac{\mathbb{d}C}{\mathbb{d}l} = {{\frac{\mathbb{d}C}{\mathbb{d}t} \cdot \frac{\mathbb{d}t}{\mathbb{d}l}} = {\frac{\overset{.}{C}}{U} = {\frac{\overset{.}{\omega}}{U^{2}} - {\frac{\omega \cdot \overset{.}{U}}{U^{3}}.}}}}}} & (17)\end{matrix}$

The system equations for the second Kalman filter 54, i.e. the curvaturefilter, that generates curvature estimates Ĉ₀ _(k|k) and Ĉ₁ _(k|k) , aregiven byx _(k+1) ^(C) F _(k) ^(C) ·x _(k) ^(C) +w _(k) ^(C) , w _(k) ^(C) ˜N(0,Q _(k) ^(C))  (18)z _(k) ^(C) =H ^(C) ·x _(k) ^(C) +v _(k) ^(C) , v _(k) ^(C) ˜N(0, R _(k)^(C)  (19)where $\begin{matrix}{{F_{k}^{C} = \begin{bmatrix}1 & {{\Delta\quad{t \cdot \hat{U}}} + {{\hat{\overset{.}{U}} \cdot \Delta}\quad{t^{2}/2}}} \\0 & 1\end{bmatrix}},{H^{C} = \begin{bmatrix}1 & 0 \\0 & 1\end{bmatrix}},{{\underset{\_}{x}}_{k}^{C} = \begin{bmatrix}C_{0} \\C_{1}\end{bmatrix}_{k}},} & (20)\end{matrix}$Δt is the update time period of the second Kalman filter 54, and thevalues of the elements of the measurement vector z _(k) ^(c) are givenby the corresponding values of the state variables—i.e. the clothoidparameters C₀ and C₁—of the curvature filter.

The measurement, z _(k) ^(c), is transformed from the estimated state[Û, {dot over (Û)}, {circumflex over (ω)}, {dot over ({circumflex over(ω)})}]_(k) ^(T) as follows: $\begin{matrix}{{\underset{\_}{z}}_{k}^{C} = \begin{bmatrix}\frac{\hat{\omega}}{\hat{U}} \\{\frac{\hat{\overset{.}{\omega}}}{{\hat{U}}^{2}} - \frac{\hat{\omega} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{3}}}\end{bmatrix}_{k❘k}} & (21)\end{matrix}$and the associated covariance of the measurements is given by:R _(k) ^(C) =J _(k) ^(C) P _(k|k) ^(h)(J _(k) ^(C))^(T)  (22)where $\begin{matrix}{J_{k}^{C} = {{\frac{\partial\begin{bmatrix}C_{0} \\C_{1}\end{bmatrix}}{\partial{\underset{\_}{x}}^{h}}❘_{{\underset{\_}{x}}^{h} = {\hat{\underset{\_}{x}}}_{k❘k}^{h}}} = {\begin{bmatrix}{- \frac{\hat{\omega}}{{\hat{U}}^{2}}} & 0 & \frac{1}{\hat{U}} & 0 \\{{- \frac{2 \cdot \hat{\overset{.}{\omega}}}{{\hat{U}}^{3}}} + \frac{3 \cdot \hat{\omega} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{4}}} & {- \frac{\hat{\omega}}{{\hat{U}}^{3}}} & {- \frac{\hat{\overset{.}{U}}}{{\hat{U}}^{3}}} & \frac{1}{{\hat{U}}^{2}}\end{bmatrix}_{k❘k}.}}} & (23)\end{matrix}$

It should be understood that other systems and methods for estimatingthe curvature parameters of the roadway 34 may be substituted in theroad curvature estimation subsystem 42 for that described above. Forexample, the curvature parameters of the roadway may also be estimatedfrom images of the roadway 34 by a vision system, either instead of orin conjunction with the above described system based upon measurementsof speed U^(h) and yaw rate ω^(h) from associated motion sensors.Furthermore, it should be understood that yaw rate can be eithermeasured or determined in a variety of ways, or using a variety ofmeans, for example, but not limited to, using a yaw gyro sensor, asteering angle sensor, a differential wheel speed sensor, or a GPS-basedsensor; a combination thereof; or functions of measurements therefrom(e.g. a function of, inter alia, steering angle rate).

Referring again to FIG. 5, in step (508), the measurements of targetrange r, range rate {dot over (r)}, and azimuth angle η are read fromthe radar processor 22, and are used as inputs to an extended Kalmanfilter 56, i.e. the main filter, which, in step (510), generatesestimates of the unconstrained target state—i.e. the kinematic statevariables of the target—which estimates are relative values in the localcoordinate system of the host vehicle 12 (i.e. the host-fixed coordinatesystem) which moves with therewith. In step (512), the unconstrainedtarget state, i.e. the target velocity and acceleration, is transformedto absolute coordinates of the absolute coordinate system fixed on thehost vehicle 12 at the current instant of time as illustrated in FIG. 3,so as to be consistent with the absolute coordinate system in which theroad constraint equations are derived and for which the associatedcurvature parameters are assumed to be constant, when used in theassociated constraint equations described hereinbelow in order togenerate estimates of the constrained target state. The absolutecoordinate system superimposes the moving coordinate system in space atthe current instant of time, so that the transformation in step (512) isrealized by adding velocity and acceleration related correctionterms—accounting for the motion of the host vehicle 12—to thecorresponding target estimates, in both x and y directions.

The result from the coordinate transformation in step (512) of theoutput from the extended Kalman filter 56 is then partitioned into thefollowing parts, corresponding respectively to the x and y position ofthe target vehicle 36 relative to the host vehicle 12, wherein thesuperscript 1 refers to the unconstrained target state of the targetvehicle 36: $\begin{matrix}{{\underset{\_}{\hat{X}}}_{t_{k❘k}}^{1} = {{\begin{bmatrix}{\hat{\underset{\_}{x}}}_{t}^{1} \\{\hat{\underset{\_}{y}}}_{t}^{1}\end{bmatrix}_{k❘k}\quad{and}\quad P_{t_{k❘k}}^{1}} = {\begin{bmatrix}P_{x_{t}} & P_{{xy}_{t}}^{1} \\P_{{yx}_{t}}^{1} & P_{y_{t}}^{1}\end{bmatrix}_{k❘k}.}}} & (24)\end{matrix}$

Referring again to FIG. 5, following steps (506) and (512), in steps(514) through (524) described more fully hereinbelow, variousconstraints on the possible trajectory of the target vehicle 36 areapplied and tested to determine if the target vehicle 36 is likelytraveling in accordance with one of the possible constraints. Forexample, the constraints are assumed to be from a set of lanes thatincludes the host lane 38 and possible neighboring lanes 40, and atarget vehicle 36 that is likely traveling in accordance with one of thepossible constraints would likely be traveling on either the host lane38 or one of the possible neighboring lanes 40. In step (524), thehypothesis that the target vehicle 36 is traveling on either the hostlane 38 or one of the possible neighboring lanes 40 is tested for eachpossible lane. If the hypothesis is not satisfied for one of thepossible lanes, then, in step (526), the state of the target is assumedto be the unconstrained target state, which is then used for subsequentpredictive crash sensing analysis and control responsive thereto.Otherwise, from step (524), in step (528), the target state iscalculated by the target state fusion subsystem 50 as the fusion of theunconstrained target state with the associated state of the constraintthat was identified in step (524) as being most likely.

Prior to discussing the process of steps (514) through (524) fordetermining whether the target is likely constrained by a constraint,and if so, what is the most likely constraint, the process of fusing theunconstrained target state with state of a constraint will first bedescribed for the case of a target vehicle 36 moving in the same lane asthe host vehicle 12. The constraints are applied in the y-direction andare derived from road equations where y-direction state variables arefunctions of x-direction state variables, consistent with theassumptions that the host vehicle 12 moves along the center line 41 ofits lane 38 steadily without in-lane wandering and that the roadcurvatures of all the parallel lanes 38, 40 are the same, and given thatthe absolute coordinate system is fixed on the host vehicle 12 at thecurrent instant of time. Assuming the target vehicle 36 is moving in thesame lane 38 as the host vehicle 12, and using the road constraintequation with the estimated coefficients (or parameters), in step (514),the constraint state variables are then given in terms of the lateralkinematic variable as: $\begin{matrix}{{{\hat{\overset{\_}{\underset{\_}{y}}}}_{t_{k❘k}} = {\begin{bmatrix}\overset{\_}{y} \\\overset{\_}{\overset{.}{y}} \\\overset{\_}{\overset{¨}{y}}\end{bmatrix}_{t_{k❘k}} = \begin{bmatrix}{{{{\hat{C}}_{0}\left( {\hat{x}}^{1} \right)}^{2}/2} + {{{\hat{C}}_{1}\left( {\hat{x}}^{1} \right)}^{3}/6}} \\{{{\hat{C}}_{0}{\hat{x}}^{1}{\hat{\overset{.}{x}}}^{1}} + {{{\hat{C}}_{1}\left( {\hat{x}}^{1} \right)}^{2}{{\hat{\overset{.}{x}}}^{1}/2}}} \\{{{\hat{C}}_{0}\left( {\hat{\overset{.}{x}}}^{1} \right)}^{2} + {{\hat{C}}_{0}{\hat{x}}^{1}{\hat{\overset{¨}{x}}}^{1}} + {{\hat{C}}_{1}{{\hat{x}}^{1}\left( {\hat{\overset{.}{x}}}^{1} \right)}^{2}} + {{{\hat{C}}_{1}\left( {\hat{x}}^{1} \right)}^{2}{{\hat{\overset{¨}{x}}}^{1}/2}}}\end{bmatrix}}}{and}} & (25) \\{{{\overset{\_}{P}}_{{yt}_{k❘k}} = {{A_{k}^{1}{P_{{xt}_{k❘k}}\left( A_{k}^{1} \right)}^{T}} + {A_{k}^{2}{P_{k❘k}^{C}\left( A_{k}^{2} \right)}^{T}}}}{where}} & (26) \\{{A_{k}^{1} = \begin{bmatrix}{{{\hat{C}}_{0}{\hat{x}}^{1}} + {{{\hat{C}}_{1}\left( {\hat{x}}^{1} \right)}^{2}/2}} & 0 & 0 \\{{{\hat{C}}_{0}{\hat{\overset{.}{x}}}^{1}} + {{\hat{C}}_{1}{\hat{x}}^{1}{\hat{\overset{.}{x}}}^{1}}} & {{{\hat{C}}_{0}{\hat{x}}^{1}} + {{{\hat{C}}_{1}\left( {\hat{x}}^{1} \right)}^{2}/2}} & 0 \\{{{\hat{C}}_{0}{\hat{\overset{¨}{x}}}^{1}} + {{\hat{C}}_{1}\left( {\hat{\overset{.}{x}}}^{1} \right)}^{2} + {{\hat{C}}_{1}{\hat{x}}^{1}{\hat{\overset{¨}{x}}}^{1}}} & {{2{\hat{C}}_{0}\hat{\overset{.}{x}}} + {2{\hat{C}}_{1}{\hat{x}}^{1}\hat{\overset{.}{x}}}} & {{{\hat{C}}_{0}{\hat{x}}^{1}} + {{{\hat{C}}_{1}\left( {\hat{x}}^{1} \right)}^{2}/2}}\end{bmatrix}}{and}} & (27) \\{A_{k}^{2} = {\begin{bmatrix}{\left( {\hat{x}}^{1} \right)^{2}/2} & {\left( {\hat{x}}^{1} \right)^{3}/6} \\{{\hat{x}}^{1} \cdot {\hat{\overset{.}{x}}}^{1}} & {\left( {\hat{x}}^{1} \right)^{2} \cdot {{\hat{\overset{.}{x}}}^{1}/2}} \\{\left( {\hat{\overset{.}{x}}}^{1} \right)^{2} + {{\hat{x}}^{1} \cdot {\hat{\overset{¨}{x}}}^{1}}} & {{{\hat{x}}^{1} \cdot \left( {\hat{\overset{.}{x}}}^{1} \right)^{2}} + {\left( {\hat{x}}^{1} \right)^{2} \cdot {{\hat{\overset{¨}{x}}}^{1}/2}}}\end{bmatrix}.}} & (28)\end{matrix}$

In step (528), the two y-coordinate estimates, one from the main filterand the other from the road constraint, are then fused as follows:$\begin{matrix}{P_{{yt}_{k❘k}}^{f} = {{\text{[}\text{(}{\overset{\_}{P}}_{{yt}_{k❘k}}\text{)}^{- 1}} + {\text{(}P_{{yt}_{k❘k}}^{1}\text{)}^{- 1}\text{]}^{- 1}}}} & (29) \\{{\hat{\underset{\_}{y}}}_{t_{k❘k}}^{f} = {{P_{{yt}_{k❘k}}^{f}\text{[}\left( {\overset{\_}{P}}_{{yt}_{k❘k}} \right)^{- 1}{\hat{\overset{\_}{\underset{\_}{y}}}}_{t_{k❘k}}} + {\text{(}P_{{yt}_{k❘k}}^{1}\text{)}^{- 1}{\hat{\underset{\_}{y}}}_{t_{k❘k}}^{1}\text{]}}}} & (30)\end{matrix}$

Finally, the composed estimate of the target state is $\begin{matrix}{{{\hat{\underset{\_}{X}}}_{t_{k❘k}} = \begin{bmatrix}{\hat{\underset{\_}{x}}}_{t_{k❘k}}^{1} \\{\hat{\underset{\_}{y}}}_{t_{k❘k}}^{f}\end{bmatrix}}{and}} & (31) \\{P_{t_{k❘k}} = {\begin{bmatrix}P_{x_{t}} & {\overset{\_}{P}}_{{xy}_{t}} \\{\overset{\_}{P}}_{{xy}_{t}}^{\prime} & P_{y_{t}}^{f}\end{bmatrix}_{k❘k}.}} & (32)\end{matrix}$where{overscore (P)} _(xy) _(l) =P _(x) _(l) (A _(k) ¹)′  (33)

In step (530), this composed estimate would then be output as theestimate of the target state if the target vehicle 36 were to bedetermined from steps (514) through (524) to be traveling in the hostlane 38.

Returning to the process of steps (514) through (524) for determiningwhether the target is likely constrained by a constraint, and if so,what is the most likely constraint; according to the assumption thattargets follow the same roadway 34, if the target vehicle 36 were knownto travel in a particular lane, it would desirable to use estimated roadparameters for that lane as a constraint in the main filter ofestimating target kinematics. However, the knowledge of which lane thetarget vehicle 36 is current in is generally not available, especiallywhen the target is moving on a curved roadway 34. Since the roadequation (8) is only for the host lane 38 in the host-centeredcoordinate system, constrained filtering would require knowing whichlane the target is in, and different constraint equations would beneeded for different lanes. Ignoring the difference of road curvatureparameters among these parallel lanes, i.e. assuming the curvature ofeach lane to be the same, the road equation for an arbitrary lane can bewritten as: $\begin{matrix}{{y = {{mB} + {{\hat{C}}_{0}\frac{x^{2}}{2}} + {{\hat{C}}_{1}\frac{x^{3}}{6}}}},{m = 0},{\pm 1},{\pm 2},\ldots} & (34)\end{matrix}$where B is the width of the lanes and m represents the lane to bedescribed (m=0 corresponds the host lane 38, m=1 corresponds the rightneighboring lane 40, m=−1 corresponds the left neighboring lane 40, andso on). Without the prior knowledge of the target lane position, each ofthe multiple constraints forming a multiple constraint system (analogousto the so-called multiple model system) is tested to determine which, ifany, of the constraints are active. A multiple constraint (MC) system issubjected to one of a finite number N^(C) of constraints. Only oneconstraint can be in effect at any given time. Such systems are referredto as hybrid—they have both continuous (noise) state variables as wellas discrete number of constraints.

The following definitions and modeling assumptions are made tofacilitate the solution of this problem:

Constraint equations:y _(t) _(k) =f _(t) _(k) ( x _(t) _(k) )  (35)

-   -   where f _(t) _(k) denotes the constraint at time t_(k) in effect        during the sampling period ending at t_(k).

Constraint: among the possible N^(C) constraintsf _(t) _(k) ∈{f ^(j)}_(j=1) ^(N) ^(C)   (36)

-   -   {overscore (ŷ)} _(t) _(k|kphu j) : state estimate at time t_(k)        using constraint f _(t) _(k) ^(j)    -   {overscore (P)}_(yt) _(k|k) ^(j), {overscore (P)}_(xyt) _(k|k)        ^(j): covariance matrix at time t_(k) under constraint f _(t)        _(k) ^(j)    -   μ_(t) _(k−1) ^(j): probability that the target is following        constraint j at time t_(k−1)

Constraint jump process: is a Markov chain with known transitionprobabilitiesP{f _(t) _(k) =f ^(j) |f _(t) _(k−1) =f ^(i) }=p _(ij).  (37)

To implement the Markov model—for systems with more than one possibleconstraint state—it is assumed that at each scan time there is aprobability p_(ij) that the target will make the transition fromconstraint state i to state j. These probabilities are assumed to beknown a priori and can be expressed in the probability transition matrixas shown below. $\begin{matrix}{P_{trans} = {\begin{matrix}\quad \\\quad \\{Prior} \\{State}\end{matrix}\begin{matrix}{{New}\quad{State}} \\\begin{matrix}{\quad{1\quad 2\quad 3}} \\{\begin{matrix}1 \\2 \\3\end{matrix}\begin{bmatrix}p_{11} & p_{12} & p_{13} \\p_{21} & p_{22} & p_{23} \\p_{31} & p_{32} & p_{33}\end{bmatrix}}\end{matrix}\end{matrix}}} & (38)\end{matrix}$

The prior probability that f ^(j) is correct (f ^(j) is in effect) isP(f ^(j) |Z ⁰)=μ_(t) _(o) ^(j) j=1, . . . N ^(C)  (39)where Z⁰ is the prior information and $\begin{matrix}{{\sum\limits_{j = 1}^{N^{C}}\mu_{t_{0}}^{j}} = 1} & (40)\end{matrix}$since the correct constraint is among the assumed N^(C) possibleconstraints.

The constrained target state estimation subsystem 46 provides fordetermining whether the target state corresponds to a possibleconstrained state, and if so, then provides for determining the mostlikely constrained state.

A multiple constraint (MC) estimation algorithm mixes and updates N^(C)constraint-conditioned state estimates using the unconstrained stateestimate ŷ _(t) _(k|k) ¹ as a measurement, along with the calculation ofthe likelihood function and probability associated with each constraint.In one embodiment of the multiple constraint (MC) estimation algorithm,the constrained state estimate output is a composite combination of allof the constraint-conditioned state estimates. If this constrained stateestimate is valid, i.e. if the constrained state estimate correspondsto—e.g. matches—the unconstrained state estimate, then the target stateis given by fusing the constrained and unconstrained state estimates;otherwise the target state is given by the unconstrained state estimate.This embodiment of the multiple constraint (MC) estimation algorithmcomprises the following steps:

1. Estimation of state variables from multiple constraints: In step(514), using the multiple lane road equation (34) to replace the firstrow in equation (25), the multiple constraint state estimates are givenby: $\begin{matrix}\begin{matrix}{{\hat{\underset{\_}{\overset{\_}{y}}}}_{t_{k❘k}}^{0j} = \begin{bmatrix}\overset{\_}{y} \\\overset{\_}{\overset{.}{y}} \\\overset{\_}{\overset{¨}{y}}\end{bmatrix}_{t_{k❘k}}} \\{= \begin{bmatrix}{B_{j} + {{\hat{C}}_{0} \cdot {\left( {\hat{x}}^{1} \right)^{2}/2}} + {{\hat{C}}_{1} \cdot {\left( {\hat{x}}^{1} \right)^{3}/6}}} \\{{{\hat{C}}_{0} \cdot {\hat{x}}^{1} \cdot {\hat{\overset{.}{x}}}^{1}} + {{\hat{C}}_{1} \cdot \left( {\hat{x}}^{1} \right)^{2} \cdot {{\hat{\overset{.}{x}}}^{1}/2}}} \\{{{\hat{C}}_{0} \cdot \left( {\hat{\overset{.}{x}}}^{1} \right)^{2}} + {{\hat{C}}_{0} \cdot {\hat{x}}^{1} \cdot {\hat{\overset{¨}{x}}}^{1}} + {{\hat{C}}_{1} \cdot {\hat{x}}^{1} \cdot}} \\{\left( {\hat{\overset{.}{x}}}^{1} \right)^{2} + {{\hat{C}}_{1} \cdot \left( {\hat{x}}^{1} \right)^{2} \cdot {{\hat{\overset{¨}{x}}}^{1}/2}}}\end{bmatrix}}\end{matrix} & (41)\end{matrix}$where ${B_{j} = 0},{\pm B},\ldots\quad,{{\pm \frac{N^{C} - 1}{2}}B},$and B is the width of a lane. Stated in another way, the constraintstate estimates corresponds to—e.g. matches—the y locations of thecenterlines of each possible lane in which the target vehicle 36 couldbe located.

The associated covariance is given by:P _(yt) _(k|k) ^(0j) =A _(k) ¹ ·P _(xt) _(k|k) ·(A _(k) ¹)^(T) +A _(k) ²·P _(k|k) ^(C)·(A _(k) ²)^(T)  (42)where A_(k) ¹ and A_(k) ² are given by equation (27) and equation (28),P_(xt) _(k|k) is from equation (24) and P_(k|k) ^(C) is from thecurvature filter.

2. Constraint-conditioned updating: In step (516), the state estimatesand covariance conditioned on a constraint being in effect are updated,as well as the constraint likelihood function, for each of theconstraints j=1, . . . N^(C). The updated state estimate and covariancescorresponding to constraint j are obtained using measurement ŷ _(t)_(k|k) ¹, as follows:{overscore (ŷ)} _(t) _(k|k) ^(j) ={overscore (ŷ)} _(t) _(k|k) ^(0j)+{overscore (P)} _(yt) _(k|k) ^(0j)({overscore (P)} _(yt) _(k|k) ^(0j)+P _(yt) _(k|k) ¹)⁻¹( ŷ _(t) _(k|k) ¹ −{overscore (ŷ)} _(t) _(k|k)^(0j))  (43){overscore (P)} _(yt) _(k|k) ^(j) ={overscore (P)} _(yt) _(k|k) ^(0j)−{overscore (P)} _(yt) _(k|k) ^(0j)({overscore (P)} _(yt) _(k|k) ^(0j)+P _(yt) _(k|k) ¹)⁻¹({overscore (P)} _(yt) _(k|k) ^(0j).  (44){overscore (P)} _(xyt) _(k|k) ^(j) ={overscore (P)} _(xyt) _(k|k) ^(0j)−{overscore (P)} _(yt) _(k|k) ^(0j)({overscore (P)} _(yt) _(k|k) ^(0j)+{overscore (P)} _(yt) _(k|k) ¹)⁻¹ {overscore (P)} _(xyt) _(k|k)^(0j)  (45)

3. Likelihood calculation: In step (518), the likelihood functioncorresponding to constraint s is evaluated at the value y _(t) _(k|k) ¹of the unconstrained target state estimate, assuming a Gaussiandistribution of the measurement around the constraint-conditioned stateestimate for each of the constraints j=1, . . . N^(C), as follows:Λ_(t) _(k) ^(j) =N( y _(t) _(k|k) ¹ ; {overscore (ŷ)} _(t) _(k|k) ^(0j), {overscore (P)} _(yt) _(k|k) ^(0j) +P _(yt) _(k|k) ¹)  (46)wherein the Gaussian distribution N( ; , ) has a mean value of{overscore (ŷ)} _(t) _(k|k) ^(0j) and an associated covariance of{overscore (P)}_(yt) _(k|k) ^(0j)+P_(yt) _(k|k) ¹.

4. Constraint probability evaluations: In step (520), the updatedconstraint probabilities are calculated for each of the constraints j=1,. . . N^(C), as follows: $\begin{matrix}{\mu_{t_{k}}^{j} = {\frac{1}{a}\Lambda_{t_{k}}^{j}{\overset{\_}{a}}_{j}}} & (47)\end{matrix}$where {overscore (a)}_(j), the probability after transition thatconstraint j is in effect, is given by $\begin{matrix}{{\overset{\_}{a}}_{j} = {\sum\limits_{i = 1}^{N^{C}}{p_{ij} \cdot \mu_{t_{k - 1}}^{i}}}} & (48)\end{matrix}$and the normalizing constant is $\begin{matrix}{a = {\sum\limits_{j = 1}^{N^{C}}{\Lambda_{t_{k}}^{j}{{\overset{\_}{a}}_{j}.}}}} & (49)\end{matrix}$

5. Overall state estimate and covariance: In step (522), the combinationof the latest constraint-conditioned state estimates and covariances isgiven by: $\begin{matrix}{{\hat{\underset{\_}{\overset{\_}{y}}}}_{t_{k❘k}} = {\sum\limits_{j = 1}^{N^{C}}{\mu_{t_{k}}^{j} \cdot {\hat{\underset{\_}{\overset{\_}{y}}}}_{t_{k❘k}}^{j}}}} & (50) \\{{\overset{\_}{P}}_{{yt}_{k❘k}} = {\sum\limits_{j = 1}^{N^{C}}{\mu_{t_{k}}^{j} \cdot {\left\lbrack {{\overset{\_}{P}}_{{yt}_{k❘k}}^{j} + {\left( {{\hat{\overset{\_}{\underset{\_}{y}}}}_{t_{k❘k}}^{j} - {\hat{\overset{\_}{\underset{\_}{y}}}}_{t_{k❘k}}} \right) \cdot \left( {{\hat{\underset{\_}{\overset{\_}{y}}}}_{t_{k❘k}}^{j} - {\hat{\underset{\_}{\overset{\_}{y}}}}_{t_{k❘k}}} \right)^{\prime}}} \right\rbrack.}}}} & (51) \\{{\overset{\_}{P}}_{{xyt}_{k❘k}} = {\sum\limits_{j = 1}^{N^{C}}{\mu_{t_{k}}^{j} \cdot {\overset{\_}{P}}_{{xyt}_{k❘k}}^{j}}}} & (52)\end{matrix}$

The output of the estimator from step (522) in the above algorithm isthen used as the constrained estimates in the fusion process describedby equations (29) and (30), and the result of equation (52), instead ofthe result of equation (33), is used in equation (32).

When the target vehicle 36 is not following the roadway 34 or ischanging lanes, imposing the road constraint on target kinematic statevariables will result in incorrect estimates that would be worse thanusing the associated unconstrained estimates. However, noise relatedestimation errors might cause a correct road constraint to appearinvalid. Accordingly, it is beneficial to incorporate a means that cankeep the constraints in effect when they are valid, e.g. when the targetvehicle 36 follows a particular lane; and lift them off promptly whenthey are invalid, e.g. when the target vehicle 36 departs from its lane.The unconstrained target state estimate plays a useful role in roadconstraint validation, since it provides independent target stateestimates.

One approach is to test the hypothesis that the unconstrained targetstate estimate satisfies the road constraint equation, or equivalently,that the constrained estimate and the unconstrained estimate eachcorrespond to the same target. The optimal test would require using allavailable target state estimates in history through time t_(k) and isgenerally not practical. A practical approach is the sequentialhypothesis testing in which the test is carried out based on the mostrecent state estimates only. In accordance with the notation usedhereinabove, the difference between the constrained and unconstrainedtarget state estimates (y direction only) is denoted:{circumflex over (δ)} _(t) _(k) =ŷ _(t) _(k|k) ¹ −{overscore (ŷ)} _(t)_(k|k)   (53)as the estimate of{overscore (δ)}_(t) _(k) =y _(f) _(k) ¹ −{overscore (y)} _(t) _(k)  (54)where y _(t) _(k) ¹ is the true target state and {overscore (y)} _(t)_(k) is the true state of a target moving along the roadway 34 (or alane). In step (524), the “same target” hypothesis is tested, i.e.H ₀: δ _(t) _(k) =0  (55)vs.H₁: δ _(t) _(k) ≠0  (56)The main filter error{tilde over (y)} _(t) _(k) ¹ =y _(t) _(k) ¹ −ŷ _(t) _(k|k) ¹  (57)is assumed independent of the error{overscore ({tilde over (y)})} _(t) _(k) ={overscore (y)} _(t) _(k)−{overscore (ŷ)} _(t) _(k|k)   (58)which is from the constraints. The covariance of the difference{circumflex over (δ)} _(t) _(k) is, under hypothesis H₀, given by:$\begin{matrix}\begin{matrix}{P_{t_{k}}^{\delta} = {E\left( {{\underset{\_}{\overset{\sim}{\delta}}}_{t_{k}}{\underset{\_}{\overset{\sim}{\delta}}}_{t_{k}}^{\prime}} \right)}} \\{= {E\left\lbrack {\left( {{\underset{\_}{\overset{\sim}{y}}}_{t_{k}}^{1} - {\overset{\sim}{\underset{\_}{\overset{\_}{y}}}}_{t_{k}}} \right)\left( {{\underset{\_}{\overset{\sim}{y}}}_{t_{k}}^{1} - {\overset{\sim}{\overset{\_}{\underset{\_}{y}}}}_{t_{k}}} \right)^{\prime}} \right\rbrack}} \\{= {P_{{yt}_{k❘k}}^{1} + {\overset{\_}{P}}_{{yt}_{k❘k}}}}\end{matrix} & (59)\end{matrix}$

Assuming that the estimation errors are Gaussian, the test of H₀ vs. H₁is as follows:Accept H ₀ if ρ_(t) _(k) ={circumflex over (δ′)} _(t) _(k) ^(t)(P _(t)_(k) ^(δ))⁻¹ {circumflex over (δ)} _(t) _(k) ≦γ  (60)

The threshold is chosen such thatP(ρ_(t) _(k) >γ|H ₀)=α  (61)where α is a predefined error tolerance value. Note that based on theabove Gaussian error assumption, ρ₁ _(k) has a chi-square distributionwith n_(y) degrees of freedom. The choice of this threshold is asignificant design factor and should be based on specific applicationneed. In road vehicle collision prediction, a target in the host lane 38is regarded to be on a collision course and is considered more dangerousthan a target in one of the neighboring lanes 40. Thus it is desirableto have a high threshold (a low error tolerance value) for a target inhost lane 38 since constrained filtering can provide accurate targetstate estimates while a “changing lane” maneuver of such a target willnot pose a threat to the host vehicle 12. On the other hand, targets inneighboring lanes 40 are usually regarded as passing-by vehicles. Thoughconstrained filtering may further reduce false alarm rate, a “changinglane” maneuver of such a target (into the host lane 38) would pose areal threat to the host vehicle 12. Thus it is desirable to have a lowthreshold (a high error tolerance value) for a target in a neighboringlane if false alarm rate is already low enough.

Based on the above analysis, the hypothesis testing scheme efficientlyuses different threshold values for targets in different lanes, with themultiple constraint filtering algorithm providing the knowledge of whichlane the target is most likely in currently. Assuming that there areN^(C) possible lanes on the roadway 34, and each lane is described by aconstraint equation, the constraint equation with the highestprobability μ_(t) _(k) ^(j) for a target corresponds to the lane thatthis target in most likely in at time t_(k) (the current time). Denotingthis most likely lane as l_(t), then $\begin{matrix}{\mu_{t_{k}}^{l_{t}} = {\max\limits_{j}{\left\{ {\mu_{t_{k}}^{j},{j = 1},\ldots\quad,r} \right\}.}}} & (62)\end{matrix}$

The difference between the unconstrained state estimates and lane l_(t)constrained state estimates (y direction only), denoted as:{circumflex over (δ)} _(t) _(k) ^(l) ^(t) =ŷ _(t) _(k|k) ¹ −{overscore(ŷ)} _(t) _(k|k) ^(l) ^(t)   (63)is the estimate ofδ _(t) _(k) ^(l) ^(t) =y _(f) _(k) ¹ −+E,ovs _(t) _(k) ^(l) ^(t)   (64)where y _(t) _(k) ¹ is the true target state and {overscore (y)} _(t)_(k) ^(l) ^(t) is the true state of a target moving along lane l_(t).The test for the “same target” hypothesis is then given by:H ₀: δ _(t) _(k) ^(l) ^(t) =0  (65)vs.H ₁: δ _(t) _(k) ^(l) ^(t) ≠0  (66)

The constrained estimation error is given by:{overscore ({tilde over (y)})} _(t) _(k) ^(l) ^(t) ={overscore (y)} _(t)_(k) −{overscore (ŷ)} _(t) _(k|k) ^(l) ^(t)   (67)

Assuming that the estimation errors are independent and Gaussian, thetest of H₀ vs. H₁ becomes:Accept H ₀ if ρ_(t) _(k) ^(l) ^(t) =({circumflex over (δ)} _(t) _(k)^(l) ^(t) )′(P _(t) _(k) ^(δ) ^(t) )⁻¹ {circumflex over (δ)} _(t) _(k)^(l) ^(t) ≦γ_(l) _(t)   (68)where $\begin{matrix}\begin{matrix}{P_{t_{k}}^{\delta_{t}} = {E\left\lbrack {\left( {{\underset{\_}{\delta}}_{t_{k}}^{l_{t}} - {\underset{\_}{\hat{\delta}}}_{t_{k}}^{l_{t}}} \right)\left( {{\underset{\_}{\delta}}_{t_{k}}^{l_{t}} - {\underset{\_}{\hat{\delta}}}_{t_{k}}^{l_{t}}} \right)^{\prime}} \right\rbrack}} \\{= {E\left\lbrack {\left( {{\underset{\_}{\overset{\sim}{y}}}_{t_{k}}^{1} - {\overset{\sim}{\overset{\_}{\underset{\_}{y}}}}_{t_{k}}^{l_{t}}} \right)\left( {{\underset{\_}{\overset{\sim}{y}}}_{t_{k}}^{1} - {\overset{\sim}{\overset{\_}{\underset{\_}{y}}}}_{t_{k}}^{l_{t}}} \right)^{\prime}} \right\rbrack}} \\{= {P_{{yt}_{k❘k}}^{1} + {\overset{\_}{P}}_{{yt}_{k❘k}}^{l_{t}}}}\end{matrix} & (69)\end{matrix}$and the threshold is such thatP(ρ_(t) _(k) ^(l) ^(t) >γ_(l) _(t) |H ₀ , l _(t)=α_(l) _(t)   (70)whereγ_(l) _(t) ∈{γ_(j)}_(j=1) ^(r) and α_(l) _(t) ∈{α_(j)}_(j=1) ^(r)  (71)

Such a lane adaptive hypothesis testing scheme provides for a promptswitch of the target state estimation output to the unconstrainedestimate when the target vehicle 36 leaves its current lane, while theestimation accuracy of a target in host lane 38 is substantiallyimproved by constrained filtering.

In another embodiment of the multiple constraint (MC) estimationalgorithm, the constrained state estimate used for the hypothesistesting is the most likely of the separate constrained target stateestimates (i.e. in accordance with a “winner take all” strategy), ratherthan a composite combination of all of the constrained target stateestimates. If this most likely constrained state estimate is valid, i.e.if the most likely constrained state estimate corresponds to—e.g.matches—the unconstrained state estimate, then the target state is givenby fusing the most likely constrained state estimate and theunconstrained state estimate; otherwise the target state is given by theunconstrained state estimate.

In yet another embodiment of the multiple constraint (MC) estimationalgorithm, hypothesis tests are made for each of the constrained stateestimates. If none of the hypotheses are accepted, then the target stateis given by the unconstrained state estimate. If one of the hypothesesis accepted, then the target state is given by fusing the correspondingconstrained state estimate and the unconstrained state estimate. If morethan one hypotheses are accepted, then the most likely constrained statemay be identified by voting results from a plurality of approaches, orby repeating the hypothesis tests with different associated thresholds.

Generally, the number of constraints (i.e. the number of roadway lanes)can vary with respect to time, as can associated parameters therewith,for example, the width of the lanes of the roadway, so as to accommodatechanges in the environment of the host vehicle 12. For example, the hostvehicle 12 in one trip could travel on a one-lane road, a two-lane roadwith opposing traffic, a three-lane road with a center turn lane, a fourline road two lanes of opposing traffic, or on a multi-lane dividedfreeway.

Road vehicle tracking simulations using constrained and unconstrainedfiltering were carried out for four scenarios. In all scenarios, thehost vehicle 12 was moving at 15.5 m/s and a target vehicle 36 isapproaching on the same roadway 34 at a speed of 15.5 m/s. The initialposition of the target was 125 meters away from the host in the xdirection, and the lane width for all lanes was assumed to be 3.6meters. The measurement variance of the vehicle speed sensor was 0.02m/s and the variance of the gyroscope yaw rate measurement was 0.0063rad/s. The variances of radar range, range rate and azimuth anglemeasurements were 0.5 m, 1 m/s, and 1.5° respectively. Simulationresults were then generated from 100 Monte-Carlo runs of the associatedtracking filters.

In the first scenario, the host vehicle 12 and the target vehicle 36were moving on a straight roadway 34 (C₀=0 and C₁=0) and the targetvehicle 36 was moving toward the host vehicle 12 in the same lane. FIGS.8 a-d illustrate the target state estimation and road curvatureestimation results of the unconstrained and constrained filteringschemes, and FIG. 9 a-b illustrate the average target vehicle 36 lateralposition, velocity and acceleration RMS errors of the unconstrained andconstrained filtering schemes. The estimation errors from constrainedfiltering were substantially reduced. Before 48 radar scans, when thetarget vehicle 36 was farther than 65 meters away from the host vehicle12, constrained filtering resulted in a more than 40 percent reductionof error in target lateral velocity estimation, and a more than 60percent reduction of error in lateral acceleration estimation. When thetarget vehicle 36 was less than 65 meters away from the host vehicle 12,which is a more relevant condition for collision prediction, more than50 percent of lateral position estimation error, and more than 90percent of lateral velocity and acceleration estimation errors, werereduced by constrained filtering.

In the second scenario, the host vehicle 42 and the target vehicle 36were moving on a curved roadway 34 (C₀=−10⁻⁵ and C₁=−3×10⁻⁵) and thetarget vehicle 36 was moving toward the host vehicle 12 in the samelane. FIGS. 10 a-d illustrate the target state estimation and curvatureestimation results of the unconstrained and constrained filteringschemes, and FIGS. 11 a-b illustrate the average target vehicle 36lateral position, velocity and acceleration RMS errors of theunconstrained and constrained filtering schemes. The estimation errorsfrom constrained filtering were substantially reduced after about 48radar scans, when the target vehicle 36 was less than 65 meters awayfrom the host vehicle 12. Estimation errors were the same forconstrained and unconstrained filtering before 20 radar scans, when thetarget vehicle 36 was about 100 meters away from the host vehicle 12.For the target vehicle 36 located between 100 and 65 meters away fromthe host vehicle 12, constrained filtering resulted in about a 30percent reduction in errors of lateral velocity and accelerationestimation, and when the target vehicle 36 was less than 65 meters awayfrom the host vehicle 12, more than 50 percent of lateral positionestimation error and more than 90 percent of lateral velocity andacceleration estimation errors were reduced by constrained filtering.The lack of improvement for constrained filtering when the targetvehicle 36 was far away resulted from estimation errors of roadcurvature parameters, which caused constraint errors proportional to thedistance between host vehicle 12 and the target vehicle 36. This is moreevident in the curved roadway 34 case, where curvature estimation errorwas larger and caused more lane position ambiguity of a distant targetvehicle 36.

In the third scenario, the host vehicle 12 and the target vehicle 36were moving on a straight roadway 34 (C₀=0 and C₁=0) and the targetvehicle 36 was initially approaching in the left neighboring lane. Att=2.2 second (55 radar scans), the target vehicle 36 began to divergefrom its lane and turns toward the host lane 38, which resulted in acollision at t=4 seconds (100 radar scans). FIGS. 12 a-d illustrate thetarget state estimation results and the lateral position and velocityRMS errors of the unconstrained and constrained filtering schemes. Theerror tolerance levels for constraint validity hypothesis testing(equation (70)) were chosen as α≈1 for the host lane 38 and α=0.5 forall neighboring lanes 40. Whereas constrained filtering withoutvalidation produces substantially lower estimation errors before thetarget vehicle 36 turns away, the associated target state estimationresult was incorrect and its RMS errors were much larger than that ofunconstrained filtering after the target vehicle 36 began to turn awayfrom its lane (the left neighboring lane), implying that the roadconstraints, which become invalid after the target vehicle 36 began todiverge from its lane, were not promptly lifted off. On the other hand,the performance of constrained filtering with validation wassubstantially close to that of unconstrained filtering, producingslightly lower estimation errors before the target vehicle 36 turnsaway, and exhibiting target state estimation results and RMS errors thatwere the same as unconstrained filtering after the target vehicle 36began to turn away from its lane, implying that road constraints werepromptly lifted off after the target vehicle 36 began to diverge fromits lane.

The fourth scenario was similar to the third scenario, the onlydifference being that the vehicles were on a curved roadway 34 (C₀=−10⁻⁵and C₁=−3×10⁻⁵) instead of a straight one. The target vehicle 36 beganto diverge at t=2.2 s and results in a collision at t=4 s. FIGS. 13 a-dillustrate the target state estimation results and the lateral positionand velocity RMS errors of the unconstrained and constrained filteringschemes. The error tolerance levels were the same as in the thirdscenario, and the results and observations were also similar to that ofthe third scenario. Road constraints were promptly lifted off by theproposed constraint validation after the target vehicle 36 began todiverge from its lane. In general, the overall improvement byconstrained filtering in estimation accuracy of target vehicle 36lateral kinematics was substantial, given the fact that estimationaccuracy of target vehicle 36 lateral kinematics was often limited bypoor radar angular resolution.

Accordingly, simulation results of road vehicle tracking on bothstraight and curved roadways 34 show that the predictive collisionsensing system 10 could substantially reduce the estimation errors intarget vehicle 36 lateral kinematics when the target vehicles 36 were inthe host lane 38. When a target vehicle 36 maneuvers from a neighboringlane into the host lane 38, the predictive collision sensing system 10promptly detects this maneuver and lifts off the road constraint toavoid an otherwise incorrect constrained result. In view of the factthat poor radar angular resolution often results in poor lateralkinematics estimation, the predictive collision sensing system 10 hasprovided for a substantial improvement in estimation accuracy of targetvehicle 36 lateral kinematics, which is beneficial for an early andreliable road vehicle collision prediction.

Referring to FIG. 14, in accordance with another embodiment, thepredictive collision sensing system 10 further comprises a steer anglesensor 58 which provides a measure indicative of or responsive to thesteer angle δ of one or more steered wheels 60. For example, referringto FIG. 15, the steer angle δ of a particular steered wheel 60, e.g. oneof the front wheels, is the angle between the longitudinal axis 62 ofthe vehicle 12 and the heading direction 64 of the steered wheel 60,wherein the heading direction 64 is the direction in which the steeredwheel 60 rolls. Under cornering conditions, responsive to the steeredwheel 60 being steered by a steer angle δ, the steered wheel 60experiences a lateral slip as it rolls, which causes the resultingtravel direction 66 to differ from the heading direction 64 by anassociated slip angle α. The action between the associated tire and theroad generates a laterally directed cornering force F_(y)responsive—e.g. proportional—to the slip angle α which causes thevehicle 12 to turn, and which can be modeled as:F _(y) =C _(α)·α  (72)wherein the associated proportionality constant, C_(α)—also known ascornering stiffness—is defined as the slope of cornering force F_(y)with respect to slip angle α at α=0.

Generally, for a vehicle 12 with two laterally displaced steered wheels60, the associated steer angles δ of different steered wheels 60—i.e.inside and outside relative to a turn—will be different. However,referring to FIG. 16 a, for a vehicle 12 traveling at relatively higherlongitudinal speed U, the turn radius R is substantially larger than thewheelbase L of the vehicle 12, in which case the associated slip anglesa of the different steered wheels 60 are relatively small so that thedifference therebetween for the inside and outside steered wheels 60 canbe assumed to be negligible. Accordingly, for purposes of describingcornering behavior, the vehicle 12 can be represented by what is knownas a bicycle model 68 with a front 63 and rear 65 wheel, eachcorresponding to a composite of the associated front or rear wheels ofthe vehicle 12, wherein different steered wheels 60 at the front or rearof the vehicle are modeled as a single steered wheel 60 that is steeredat an associated steer angle δ. Each wheel of the bicycle model 68—front63 and rear 65—is assumed to generate the same lateral force responsiveto an associated slip angle α as would all (e.g. both) correspondingwheels of the actual vehicle 12.

For a vehicle 12 at a longitudinal speed U following a curved pathhaving a turn radius R, the sum of the lateral forces, i.e. corneringforces F_(y), is equal to the product of the mass M of the vehicle 12times the resulting centripetal acceleration, as follows:$\begin{matrix}{{\sum F_{y}} = {{F_{yf} + F_{yr}} = {M \cdot {U^{2}/R}}}} & (73)\end{matrix}$where F_(yf) and F_(yr) are the lateral forces at the front 63 and rear65 wheels respectively.

Assuming that the yaw rotational acceleration of the vehicle 12 aboutthe center of gravity CG is negligible, the sum of the moments caused bythe front and rear lateral forces is equal to zero, with the result thatF _(yf) =F _(yr) ·c/b  (74)where b and c are the distances from the center of gravity CG to thefront 63 and rear 65 wheels respectively.

The lateral force F_(yr) at the rear wheel 65 is then given as followsby substituting equation (74) into equation (73): $\begin{matrix}{F_{yr} = {{\left( {M \cdot {b/L}} \right) \cdot \left( {U^{2}/R} \right)} = {\frac{W_{r}}{g} \cdot \frac{U^{2}}{R}}}} & (75)\end{matrix}$where b and c are the distances from the center of gravity CG to thefront 63 and rear 65 wheels respectively, and W_(r) is the weight of thevehicle 12 carried by the rear wheel 65. Accordingly, the lateral forceF_(yr) at the rear wheel 65 is given by the product of the portion ofvehicle mass (W_(r)/g) carried by the rear wheel 65 times the lateralacceleration at the rear wheel 65.

Similarly, the lateral force F_(yf) at the front wheel 63 is given bythe product of the portion of vehicle mass (W_(f)/g) carried by thefront wheel 63 times the lateral acceleration at the front wheel 63 asfollows: $\begin{matrix}{F_{y\quad r} = {{\left( {M \cdot {c/L}} \right) \cdot \left( {U^{2}/R} \right)} = {\frac{W_{f}}{g} \cdot \frac{U^{2}}{R}}}} & (76)\end{matrix}$where W_(f) is the weight of the vehicle 12 carried by the front wheel63.

Given the lateral forces F_(yf), F_(yr) at the front 63 and rear 65wheels respectively, the associated slip angles α_(f), α_(r) are givenfrom equations (72), (75) and (76) as follows:α_(f) =W _(f) ·U ² /C _(αf) ·g·R)  (77)andα_(r) =W _(r) ·U ²/(C _(αr) ·g·R)  (78)From the geometry illustrated in FIG. 16 b, the steer angle δ is givenby:δ=L/R+α _(f)−α_(r)  (79)

Substituting for α_(f) and α_(r) in equation (79) from equations (77)and (78) gives: $\begin{matrix}{\delta = {\frac{L}{R} + {\left( {\frac{W_{f}}{C_{\alpha\quad f}} - \frac{W_{r}}{C_{\alpha\quad r}}} \right) \cdot \frac{U^{2}}{g \cdot R}}}} & (80)\end{matrix}$which can also be expressed as:δ=L/R+K·α _(y)  (81)where

-   -   δ=Steer angle at the front wheels (rad)    -   L=Wheelbase (m)    -   R=Turn Radius (m)    -   U=Longitudinal speed (m/sec)    -   g=Gravitational acceleration constant=9.81 m/sec²    -   W_(f)=Load on the front axle (kg)    -   W_(r)=Load on the rear axle (kg)    -   C_(αf)=Cornering stiffness of the front tires (kg_(y)/rad)    -   C_(αr)=Cornering stiffness of the rear tires (kg_(y)/rad)    -   K=understeer gradient (rad/g)    -   a_(y)=lateral acceleration (g)

Equations (80) and (81) describe the relationship between steer angle δand lateral acceleration a_(y)=U²/(gR). The factorK=[W_(f)/C_(αf)−W_(r)/C_(αr)]—which provides the sensitivity of steerangle δ to lateral acceleration a_(y), and which is also referred to asan understeer gradient—consists of two terms, each of which is the ratioof the load on the wheel W_(f), W_(r) (front or rear) to thecorresponding cornering stiffness C_(αf), C_(αr) of the associatedtires. Depending upon the value of the understeer gradient K, thecornering behavior of the vehicle 12 is classified as either neutralsteer, understeer or oversteer, depending upon whether K is equal tozero, greater than zero, or less than zero, respectively.

For a vehicle 12 exhibiting neutral steer,W _(f) /C _(αf) =W _(r) /C _(αr) →K=0→α_(f)=α_(r)  (82a)so that, for a constant-radius turn, there would be no change in steerangle δ as the longitudinal speed U is varied.

For a vehicle 12 exhibiting understeer,W _(f) /C _(αf) >W _(r) /C _(αr) →K>0→α_(f)>α_(r)  (82b)so that, for a constant-radius turn, the steer angle δ would need toincrease with increasing longitudinal speed U in proportion to theproduct of the understeer gradient K times the lateral accelerationa_(y).

For a vehicle 12 exhibiting oversteer,W _(f)/C_(αf) <W _(r)/C_(αr)→K<0→α_(f)<α_(r)  (82c)so that, for a constant-radius turn, the steer angle δ would need todecrease with increasing longitudinal speed U in proportion to theproduct of the understeer gradient K times the lateral accelerationa_(y).

A vehicle 12 steered with a steer angle δ develops a yaw rate ω that isrelated to the longitudinal speed U and turn radius R by:$\begin{matrix}{\omega = {\frac{U}{R}\quad\left( {{rad}\text{/}\sec} \right)}} & (83)\end{matrix}$

Solving for the turn radius R from equation (79) and substituting inequation (83) gives the following relationship between yaw rate ω andsteer angle δ: $\begin{matrix}{\omega = {\frac{\frac{U}{L}}{1 + \frac{K \cdot U^{2}}{L \cdot g}} \cdot \delta}} & (84)\end{matrix}$which can be used to find the relationship between the associated errorvariances, e.g. assuming that the vehicle 12 exhibits neutral steerbehavior. For example, in neutral steer case, K=0, so that equation (84)becomes: $\begin{matrix}{\omega = {\frac{U}{L} \cdot \delta}} & (85)\end{matrix}$

The error variance of ω is given by: $\begin{matrix}{\sigma_{w}^{2} = {{E\left\lbrack \left( {\omega - \hat{\omega}} \right)^{2} \right\rbrack} = {{E\left\lbrack \left( {\frac{U\quad\delta}{L} - \frac{\hat{U}\quad\hat{\delta}}{L}} \right)^{2} \right\rbrack} = {\frac{1}{L^{2}}\left\lbrack {{E\left( {U^{2}\delta^{2}} \right)} - {{\hat{U}}^{2}{\hat{\delta}}^{2}}} \right\rbrack}}}} & (86)\end{matrix}$

Assuming that the longitudinal speed U and steer angle δ areindependent, then $\begin{matrix}\begin{matrix}{\sigma_{\omega}^{2} = {\frac{1}{L^{2}}\left\lbrack {{{E\left( U^{2} \right)} \cdot {E\left( \delta^{2} \right)}} - {{\hat{U}}^{2} \cdot {\hat{\delta}}^{2}}} \right\rbrack}} \\{= {{\frac{1}{L^{2}}\text{[}\text{(}{\hat{U}}^{2}} + {\sigma_{U}^{2}\text{)}\text{(}{\hat{\delta}}^{2}} + {\sigma_{\delta}^{2}\text{)}} - {{{\hat{U}}^{2} \cdot {\hat{\delta}}^{2}}\text{]}}}} \\{= {{\frac{1}{L^{2}}\text{[}{\hat{U}}^{2}\sigma_{\delta}^{2}} + {{\hat{\delta}}^{2}\sigma_{U}^{2}} + {\sigma_{U}^{2}\sigma_{\delta}^{2}\text{]}}}} \\{= {{\frac{1}{L^{2}}\text{(}{\hat{U}}^{2}} + {\sigma_{U}^{2}\text{)}\sigma_{\delta}^{2}} + {\frac{1}{L^{2}}{\hat{\delta}}^{2}\sigma_{U}^{2}}}}\end{matrix} & (87)\end{matrix}$

From equation (85) for a neutral steer condition $\begin{matrix}{\frac{\delta}{L} = {{\frac{\omega}{U}\quad{and}\quad\frac{\delta^{2}}{L^{2}}} = \frac{\omega^{2}}{U^{2}}}} & (88)\end{matrix}$which when substituted into equation (87) gives: $\begin{matrix}{\sigma_{\omega}^{2} = {{{\frac{1}{L^{2}} \cdot \text{(}}{\hat{U}}^{2}} + {\sigma_{U}^{2}{\text{)} \cdot \sigma_{\delta}^{2}}} + {\frac{{\hat{\omega}}^{2}}{{\hat{U}}^{2}} \cdot \sigma_{U}^{2}}}} & (89)\end{matrix}$where σ_(U) and σ_(δ) are the error variances of longitudinal speed Uand steer angle δ.

For a constant turn radius R, from equation (2), C=C₀, and fromequations (13) and (88) for a neutral steer condition, $\begin{matrix}{C_{0} = {\frac{\omega}{U} = \frac{\delta}{L}}} & (90)\end{matrix}$from which the relation between curvature error variance σ_(C0) ² andsteer angle error variance σ_(δ) ² is given by: $\begin{matrix}{\sigma_{C\quad 0}^{2} = {\frac{1}{L^{2}}\sigma_{\delta}^{2}}} & (91)\end{matrix}$

The steer angle sensor 58 can be implemented in various ways, including,but not limited to, an angular position sensor—e.g. shaft encoder,rotary potentiometer or rotary transformer/syncro—adapted to measure therotation of the steering wheel shaft or input to a steering box, e.g. apinion of a rack-and-pinion steering box; or a linear position sensoradapted to measure the position of the rack of the rack-and-pinionsteering box. The steer angle sensor 58 could be shared with anothervehicle control system, e.g. a road following or suspension controlsystem. The steer angle sensor 58 can be used to supplement a yaw ratesensor 16, and can beneficially provide independent information aboutvehicle maneuvers. Furthermore, the steer angle δ measurement error issubstantially independent of longitudinal speed U, in comparison with agyroscopic yaw rate sensor 16 for which the associated yaw rate ωmeasurement error is related to vehicle speed, notwithstanding that agyroscopic yaw rate sensor 16 is generally more accurate and moresensitive to vehicle maneuvers than a steer angle sensor 58 when each isused to generate a measure of yaw angle.

The curvature error variance associated with steer angle δ measurementscan be compared with that associated with yaw rate ω measurements inorder to identify conditions under which one measurement is moreaccurate than the other. The error variance of yaw rate ω measured witha gyroscopic yaw rate sensor 16 is given as follows:σ_(ω) ² =E[(ω _(m) −b _(m) −ω+b)² ]=E[(ω_(m)−ω)²+(b _(m)−b)²+2(ω_(m)−ω)(b _(m) −b)]  (92)σ_(m) ² =E[(ω_(m)−ω)² ]+E[(b _(m) −b)²]+2E[(ω_(m)−ω)(b _(m) −b)]  (93)σ_(ω) ²=σ_(ωm) ²+σ_(b) ²  (94)where ω is the true yaw rate, ω_(m) is the yaw rate measurement, b isthe gyro bias with drift, and b_(m) is the mean gyro bias.

The curvature error variance σ_(c0) ² of the yaw rate ω is given byequation (97), described hereinbelow. By equating equations (91) and(97), and substituting for σ_(ω) ² from equation (94), the curvatureerror variance associated with the steer angle δ measurement is equal tothe curvature error variance associated with the yaw rate ω measurementwhen: $\begin{matrix}{U = {\frac{L \cdot \sigma_{\omega}}{\sigma_{\delta}} = \frac{L\sqrt{\left( {\sigma_{\omega\quad m}^{2} + \sigma_{b}^{2}} \right)}}{\sigma_{\delta}}}} & (95)\end{matrix}$

Equation (95) defines a switching curve—e.g. as illustrated in FIG. 17,for which σ_(ωm) ²=1.68×10⁻⁵, b_(m)=2.5926, and L=3.2 meters—that can beused in one embodiment to determine whether to use steer angle δ or yawrate ω when determining road curvature parameters. For example,referring to FIG. 18, in step (502) the longitudinal speed U of thevehicle 12 is read from the speed sensor 18. Then, in step (1802), ifthe longitudinal speed U is greater than a speed threshold U^(T), thenthe road curvature parameters are determined using yaw rate ω, whereinthe speed threshold U^(T) parameter is given by equation (95) andillustrated in FIG. 17 as a function of the error variance σ_(δ) ² ofthe steer angle δ measurement, the latter of which is assumed to beconstant for a given steer angle sensor 58. More particularly, from step(1802), the yaw rate ω of the vehicle 12 is read from the yaw ratesensor 16, and then in step (506), the road curvature parameters C₀ andC₁ are estimated from (U, ω) as described hereinabove. Otherwise, fromstep (1802), if the longitudinal speed U of the vehicle 12 is less thanthe speed threshold U^(T), then in step (1804) the steer angle δ is readfrom the steer angle sensor 58, and then in step (1806), the roadcurvature parameters C₀ and C₁ are estimated from (U, δ), wherein theyaw rate ω measurement input to the first Kalman filter 52 (host statefilter) can be determined from the steer angle δ and longitudinal speedU using equation (84). If the longitudinal speed U is equal to the speedthreshold U^(T), then the estimates of the road curvature parameters C₀and C₁ from steps (506) and (1806) would have the same error variance,and either estimate could be used.

The error variances and covariance of the road curvature parameters C₀and C₁ used by the associated second Kalman filter 54, i.e. thecurvature filter, of the road curvature estimation subsystem 42generally depend upon the quantity 1/U, where U is the longitudinalspeed U of the vehicle 12. If U is a Gaussian random variable, analyticsolutions for the exact mean and variance of 1/U are not presentlyknown. Instead, U can be assumed to be a non-random variable because thevariance σ_(U) ² of U is substantially smaller than U in substantiallymost cases. Accordingly, the various variance and covariance componentsof the road curvature parameters C₀ and C₁ can be derived as follows:$\begin{matrix}\begin{matrix}{\sigma_{c\quad 0}^{2} = {E\left\{ \left\lbrack {\frac{\omega}{U} - {\hat{\omega} \cdot \left( \frac{1}{U} \right)}} \right\rbrack^{2} \right\}}} \\{= {E\left\{ {{\omega^{2} \cdot \frac{1}{U^{2}}} - {2 \cdot \omega \cdot \frac{1}{U} \cdot \hat{\omega} \cdot \left( \frac{\hat{1}}{U} \right)} + {{\hat{\omega}}^{2} \cdot \left( \frac{\hat{1}}{U} \right)^{2}}} \right\}}}\end{matrix} & (96) \\{\sigma_{c\quad 0}^{2} = {{{{E\left\lbrack \omega^{2} \right\rbrack} \cdot {E\left\lbrack \frac{1}{U^{2}} \right\rbrack}} - {{E\lbrack\omega\rbrack}^{2} \cdot {E\left\lbrack \frac{1}{U} \right\rbrack}^{2}}} \approx \frac{\sigma_{\omega}^{2}}{U^{2}}}} & (97) \\{\sigma_{c\quad 1}^{2} = \left\{ \left\lbrack {\frac{\overset{.}{\omega}}{U^{2}} - \frac{\omega\quad\overset{.}{U}}{U^{3}} - {E\left( \frac{\overset{.}{\omega}}{U^{2}} \right)} + {E\left( \frac{\omega \cdot \overset{.}{U}}{U^{3}} \right)}} \right\rbrack^{2} \right\}} & (98) \\{\sigma_{c\quad 1}^{2} = {{E\left\lbrack \left( {\frac{\overset{.}{\omega}}{U^{2}} - \frac{\omega\quad\overset{.}{U}}{U^{3}}} \right)^{2} \right\rbrack} - \left\lbrack {{E\left( \frac{\overset{.}{\omega}}{U^{2}} \right)} - {E\left( \frac{\omega\quad\overset{.}{U}}{U^{3}} \right)}} \right\rbrack^{2}}} & (99) \\{\sigma_{c\quad 1}^{2} = {{E\left\lbrack {\frac{{\overset{.}{\omega}}^{2}}{U^{4}} - {2\frac{\omega \cdot \overset{.}{\omega} \cdot \overset{.}{U}}{u^{5}}} + \frac{\omega^{2} \cdot {\overset{.}{U}}^{2}}{U^{6}}} \right\rbrack} - \quad\left\{ {\left\lbrack {E\left( \frac{\overset{.}{\omega}}{U^{2}} \right\}} \right\rbrack^{2} - {2{E\left( \frac{\overset{.}{\omega}}{U^{2}} \right)}{E\left( \frac{\omega \cdot \overset{.}{U}}{U^{3}} \right)}} + \left\lbrack {E\left( \frac{\omega \cdot \overset{.}{U}}{U^{3}} \right)} \right\rbrack^{2}} \right\}}} & (100) \\{\sigma_{c\quad 1}^{2} = {\frac{{E\left( {\overset{.}{\omega}}^{2} \right)} - {\hat{\overset{.}{\omega}}}^{2}}{{\hat{U}}^{4}} - {{2 \cdot \frac{\hat{\overset{.}{U}}}{{\hat{U}}^{5}} \cdot E}\text{(}{\omega \cdot \overset{.}{\omega}}} - {{\hat{\omega} \cdot \hat{\overset{.}{\omega}}}\text{)}} + \quad\frac{{{\left( {{\hat{\omega}}^{2} + \sigma_{\omega}^{2}} \right) \cdot \text{(}}{\hat{\overset{.}{U}}}^{2}} + {\sigma_{\overset{.}{\overset{.}{U}}}\text{)}} - {{\hat{\omega}}^{2} \cdot {\hat{\overset{.}{U}}}^{2}}}{{\hat{U}}^{6}}}} & (101) \\{\sigma_{c\quad 1}^{2} = {\frac{\sigma_{\overset{.}{\omega}}^{2}}{{\hat{U}}^{4}} + \frac{{{\hat{\omega}}^{2}\sigma_{\overset{.}{U}}^{2}} + {{\hat{\overset{.}{U}}}^{2}\sigma_{\omega}^{2}} + {\sigma_{\omega}^{2}\sigma_{\overset{.}{U}}^{2}}}{{\hat{U}}^{6}} - {2\frac{\hat{\overset{.}{U}}\quad\sigma_{\omega\overset{.}{\omega}}^{2}}{{\hat{U}}^{5}}}}} & (102) \\{\sigma_{\overset{.}{C}}^{2} = {E\left\{ \left\lbrack {\frac{\overset{.}{\omega}}{U} - \frac{\omega \cdot \overset{.}{U}}{U^{2}} - {E\left( \frac{\overset{.}{\omega}}{U} \right)} + {E\left( \frac{\omega \cdot \overset{.}{U}}{U^{2}} \right)}} \right\rbrack^{2} \right\}}} & (103) \\{\sigma_{\overset{.}{C}}^{2} = {{E\left\lbrack \left( {\frac{\overset{.}{\omega}}{U} - \frac{\omega \cdot \overset{.}{U}}{U^{2}}} \right)^{2} \right\rbrack} - \left\lbrack {{E\left( \frac{\overset{.}{\omega}}{U} \right)} - {E\left( \frac{\omega \cdot \overset{.}{U}}{U^{2}} \right)}} \right\rbrack^{2}}} & (104) \\{\sigma_{\overset{.}{C}}^{2} = {\frac{{E\left( {\overset{.}{\omega}}^{2} \right)} - {\hat{\overset{.}{\omega}}}^{2}}{{\hat{U}}^{2}} - {2\frac{{\hat{\overset{.}{U}}\text{(}E\text{(}{\omega \cdot \overset{.}{\omega}}} - {{\hat{\omega} \cdot \hat{\overset{.}{\omega}}}\text{)}}}{{\hat{U}}^{3}}} + \quad\frac{{E\text{(}{\omega^{2} \cdot {\overset{.}{U}}^{2}}\text{)}} - {{\hat{\omega}}^{2} \cdot {\hat{\overset{.}{U}}}^{2}}}{{\hat{U}}^{4}}}} & (105) \\{\sigma_{\overset{.}{C}}^{2} = {\frac{\sigma_{\overset{.}{\omega}}^{2}}{{\hat{U}}^{2}} + \frac{{{\hat{\omega}}^{2} \cdot \sigma_{\overset{.}{U}}^{2}} + {{\hat{\overset{.}{U}}}^{2} \cdot \sigma_{\omega}^{2}} + {\sigma_{\omega}^{2} \cdot \sigma_{\overset{.}{U}}^{2}}}{{\hat{U}}^{4}} - {2\frac{\hat{\overset{.}{U}} \cdot \sigma_{\omega\overset{.}{\omega}}^{2}}{{\hat{U}}^{3}}}}} & (106) \\\begin{matrix}{\sigma_{C_{0}C_{1}}^{2} = {E\left\lbrack {{\text{(}C_{0}} - {{\hat{C}}_{0}{\text{)} \cdot \text{(}}C_{1}} - {{\hat{C}}_{1}\text{)}}} \right\rbrack}} \\{= {E\left\lbrack {\left( {\frac{\omega}{U} - \frac{\hat{\omega}}{\hat{U}}} \right) \cdot \left( {\frac{\overset{.}{\omega}}{U^{2}} - \frac{\omega \cdot \overset{.}{U}}{U^{3}} - \frac{\hat{\overset{.}{\omega}}}{{\hat{U}}^{2}} + \frac{\hat{\omega} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{3}}} \right)} \right\rbrack}}\end{matrix} & (107) \\{\sigma_{C_{0}C_{1}}^{2} = {\frac{E\left( {\omega \cdot \overset{.}{\omega}} \right)}{{\hat{U}}^{3}} - \frac{\left( {{\hat{\omega}}^{2} + \sigma_{\omega}^{2}} \right) \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{4}} - \frac{\hat{\omega} \cdot \hat{\overset{.}{\omega}}}{{\hat{U}}^{3}} + \frac{{\hat{\omega}}^{2} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{4}} - \quad\frac{\hat{\omega} \cdot \hat{\overset{.}{\omega}}}{{\hat{U}}^{3}} + \frac{{\hat{\omega}}^{2} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{4}} + \frac{\hat{\omega} \cdot \hat{\overset{.}{\omega}}}{{\hat{U}}^{3}} - \frac{{\hat{\omega}}^{2} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{4}}}} & (108) \\{\sigma_{C_{0}C_{1}}^{2} = {\frac{\sigma_{\omega\overset{.}{\omega}}^{2}}{{\hat{U}}^{3}} - \frac{\hat{\overset{.}{U}}\quad\sigma_{\omega}^{2}}{{\hat{U}}^{4}}}} & (109) \\\begin{matrix}{\sigma_{C_{0}\overset{.}{C}}^{2} = {E\left\lbrack {{\text{(}C_{0}} - {{\hat{C}}_{0}{\text{)} \cdot \text{(}}\overset{.}{C}} - {\hat{\overset{.}{C}}\text{)}\text{]}}} \right.}} \\{= {E\left\lbrack {\left( {\frac{\omega}{U} - \frac{\hat{\omega}}{\hat{U}}} \right) \cdot \left( {\frac{\overset{.}{\omega}}{U} - \frac{\omega \cdot \overset{.}{U}}{U^{2}} - \frac{\hat{\overset{.}{\omega}}}{\hat{U}} + \frac{\hat{\omega} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{2}}} \right)} \right\rbrack}}\end{matrix} & (110) \\{\sigma_{C_{0}\overset{.}{C}}^{2} = {\frac{E\left( {\omega \cdot \overset{.}{\omega}} \right)}{{\hat{U}}^{2}} - \frac{\left( {{\hat{\omega}}^{2} + \sigma_{\omega}^{2}} \right) \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{3}} - \frac{\hat{\omega} \cdot \hat{\overset{.}{\omega}}}{{\hat{U}}^{2}} + \frac{{\hat{\omega}}^{2} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{3}} - \quad\frac{\hat{\omega} \cdot \hat{\overset{.}{\omega}}}{{\hat{U}}^{2}} + \frac{{\hat{\omega}}^{2} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{3}} + \frac{\hat{\omega} \cdot \hat{\overset{.}{\omega}}}{{\hat{U}}^{2}} - \frac{{\hat{\omega}}^{2} \cdot \hat{\overset{.}{U}}}{{\hat{U}}^{3}}}} & (111) \\{\sigma_{C_{0}\overset{.}{C}}^{2} = {\frac{\sigma_{\omega\overset{.}{\omega}}^{2}}{{\hat{U}}^{2}} - \frac{\hat{\overset{.}{U}}\quad \cdot \sigma_{\omega}^{2}}{{\hat{U}}^{3}}}} & (112)\end{matrix}$

Referring to FIG. 19, the road curvature estimation subsystem 42 can beembodied in a variety of ways. Generally, the road curvature estimationsubsystem 42 comprises a host state filter 52.1 and a curvature filter54.1, wherein the host state filter 52.1 processes measures responsiveto vehicle speed and vehicle yaw to determine the corresponding hoststate [ω, {dot over (ω)}, U, {dot over (U)}] of the host vehicle 12. Forexample, a speed sensor 18 provides the longitudinal speed U_(k) ^(h) ofthe vehicle 12 as the measure of vehicle speed, and either a yaw ratesensor 16 or a steer angle sensor 58, or both, provide either the yawrate ω_(k) ^(h) or the steer angle δ_(k) ^(h) respectively, or both, asthe measure of vehicle yaw, wherein samples k of the measurements areprovided by a sampled data system at corresponding sampling times. Inone embodiment, e.g. illustrated also in FIG. 4, the measure of vehicleyaw is given by the yaw rate ω_(k) ^(h) from the yaw rate sensor 16,wherein the yaw rate ω_(k) ^(h) and the longitudinal speed U_(k) ^(h)are input directly to the first Kalman filter 52 of the host statefilter 52.1. In another embodiment, the measure of vehicle yaw is givenby either the yaw rate ω_(k) ^(h) from the yaw rate sensor 16 or thesteer angle δ_(k) ^(h) from the steer angle sensor 58, depending uponthe magnitude of the longitudinal speed U_(k) ^(h) in accordance withthe process illustrated in FIG. 18 and described hereinabove. If thelongitudinal speed U of the vehicle 12 is less than the speed thresholdU^(T), then a yaw rate processor 68—e.g. embodied in the signalprocessor 26—calculates the yaw rate ω_(k) ^(h) from the steer angleδ_(k) ^(h), e.g. using equation (85) and the longitudinal speed U_(k)^(h) measurement, for input to the first Kalman filter 52. Otherwise, ifthe longitudinal speed U of the vehicle 12 is greater than or equal tothe speed threshold U^(T), the yaw rate ω_(k) ^(h) from the yaw ratesensor 16 is input to the first Kalman filter 52. In yet anotherembodiment, the steer angle sensor 58 and yaw rate processor 68 are usedwithout benefit of the yaw rate sensor 16. In yet another embodiment,the steer angle δ_(k) ^(h) from the steer angle sensor 58 is inputdirectly to the first Kalman filter 52. In yet another embodiment, ifthe longitudinal speed U of the vehicle 12 is less than the speedthreshold U^(T), then the steer angle δ_(k) ^(h) from the steer anglesensor 58 is input directly to the first Kalman filter 52 that isadapted to use steer angle δ_(k) ^(h) as the associated state variable.,and if the longitudinal speed U of the vehicle 12 is greater than orequal to the speed threshold U^(T), then the yaw rate ω_(k) ^(h) fromthe yaw rate sensor 16 is input to the first Kalman filter 52 that isadapted to use yaw rate ω_(k) ^(h) as the associated state variable.

Generally, the associated state and variance output of the host statefilter 52.1 is processed by a curvature estimator 70 so as to provideestimates of the road curvature parameters C₀ and C₁ and the errorcovariance associated therewith. Generally, the curvature estimator 70comprises a first curvature processor 72 which transforms the associatedstate and covariance output of the host state filter 52.1 to either roadcurvature parameters C₀ and C₁, or another related form—comprising ameasurement vector Z_(k) ^(c) and an associated covariance matrix R_(k)^(c) thereof—that is either used directly as the output of the roadcurvature estimation subsystem 42, or is input to a second Kalman filter54 of the curvature filter 54.1, the output of which is either used asthe output of the road curvature estimation subsystem 42, or which istransformed to road curvature parameters C₀ and C₁ and the associatedcovariance thereof using a second curvature processor 74. For example,the first 72 and second 74 curvature processors and the host statefilter 52.1 of the curvature estimator 70 can be embodied in the signalprocessor 26.

In accordance with a first embodiment, the curvature estimator 70comprises the first curvature processor 72 and the second Kalman filter54, wherein the first curvature processor 72 calculates the roadcurvature parameters C₀ and C₁ from the host state [ω, {dot over (ω)},U, {dot over (U)}]^(T) for input as a measurement vector Z_(k) ^(C) tothe second Kalman filter 54, e.g. in accordance with equation (21).Similarly, the first curvature processor 72 calculates the associatedcovariance R_(k) ^(C) of the measurement vector from the covariancep_(k|k) ^(h) of the host state vector, e.g. in accordance with equations(21) and (22). The associated second Kalman filter 54 is illustrated inFIG. 20, wherein the associated vectors and matrices referenced in theAppendix are given by: $\begin{matrix}{{\underset{\_}{Z}}_{k}^{C} = {{\begin{bmatrix}C_{0m} \\C_{1m}\end{bmatrix}_{k}\quad R_{k}^{C}} = \begin{bmatrix}\sigma_{C_{0}}^{2} & \sigma_{C_{0}C_{1}}^{2} \\\sigma_{C_{0}C_{1}}^{2} & \sigma_{C_{1}}^{2}\end{bmatrix}}} & (113) \\{F_{k} = {{\begin{bmatrix}1 & {{\hat{U}}_{k}T_{k}} \\0 & 1\end{bmatrix}\quad Q_{k}} = {\begin{bmatrix}{\text{(}{\hat{U}}_{k}T_{k}{\text{)}^{3}/3}} & {\text{(}{\hat{U}}_{k}T_{k}{\text{)}^{2}/2}} \\{\text{(}{\hat{U}}_{k}T_{k}{\text{)}^{2}/2}} & {{\hat{U}}_{k}T_{k}}\end{bmatrix} \times q}}} & (114)\end{matrix}$wherein q=6×10⁻ ⁷, T_(k) is the sampling time interval, and the varianceand covariance elements of the associated R matrix are given byequations (97), (102) and (109). The output of the road curvatureestimation subsystem 42 is then given by the output of the second Kalmanfilter 54. The first embodiment of the curvature estimator 70 is alsoillustrated in FIG. 4, wherein the action of the first curvatureprocessor 72 is implicit in the interconnection between the first 52 andsecond 54 Kalman filters thereof.

A second embodiment of the curvature estimator 70 is a modification ofthe first embodiment, wherein the second Kalman filter 54 is adapted toincorporate a sliding window in the associated filtering process. Thelength of the sliding window is adapted so as to avoid excessive delaycaused by window processing, and for example, in one embodiment,comprises about 5 samples. The associated vectors andmatrices—referenced in the Appendix—of the associated second Kalmanfilter 54 are given by: $\begin{matrix}{{F_{k} = \begin{bmatrix}1 & {d\quad L} \\0 & 1\end{bmatrix}}{R_{k}^{C} = {\begin{bmatrix}{{mean}\text{(}{\sigma_{C_{0}}^{2}\left( {k - {L\text{:}k}} \right)}\text{)}} & {{mean}\text{(}{\sigma_{C_{0}C_{1}}^{2}\left( {k - {L\text{:}k}} \right)}\text{)}} \\{{mean}\text{(}{\sigma_{C_{0}C_{1}}^{2}\left( {k - {L\text{:}k}} \right)}\text{)}} & {{mean}\text{(}{\sigma_{C_{1}}^{2}\left( {k - {L\text{:}k}} \right)}\text{)}}\end{bmatrix}/L}}} & (115) \\{{{\underset{\_}{Z}}_{k}^{C} = \begin{bmatrix}{{mean}\left( {C_{0m}\left( {k - {L\text{:}k}} \right)} \right)} \\{{mean}\left( {C_{1m}\left( {k - {L\text{:}k}} \right)} \right)}\end{bmatrix}_{k}}{Q_{k} = {\begin{bmatrix}{\left( {d\quad L} \right)^{3}/3} & {\left( {d\quad L} \right)^{2}/2} \\{\left( {d\quad L} \right)^{2}/2} & {d\quad L}\end{bmatrix} \times q}}} & (116)\end{matrix}$where L is the sliding window length,${{d\quad L} = {{{mean}\left( {U\left( {k - {L\text{:}k}} \right)} \right)} \times {\sum\limits_{i = {k - L}}^{k}T_{i}}}},$and q=2×10⁻⁷.

The variances and covariance of the associated R matrix are given byequations (97), (102) and (109).

A third embodiment of the curvature estimator 70 is a modification ofthe second embodiment, wherein the length L of the sliding window isadaptive. For example, the window length can be adapted to be responsiveto the road curvature parameters C₀ and C₁, for example, in accordancewith the following rule:L _(k)=min{max{25−floor [(|Ĉ ₀ _(—) _(k−1) |×|Ĉ ₁ _(—) _(k−1)|)×10⁴],1},L _(k−1)+1}  (117)This rule provides for a larger window length L—e.g. as large as 25samples—when both C₀ and C₁ are relatively small, for example,corresponding to a straight section of road. The window length L becomessmaller—e.g. as small as 1 sample—when either C₀ or C₁ is large,corresponding to a turn or transition of road. Furthermore, the windowlength L can be sharply reduced to account for a sudden vehiclemaneuver, with a limitation on the subsequent increase in window lengthL to one sample per step so that the previous samples so not adverselyaffect the output from the curvature estimator 70 when maneuver ends. Asthe window length L is changed, the associated F, Q, and R matrices inthe second Kalman filter 54 are also changed.

In accordance with a fourth embodiment, the curvature estimator 70comprises the first curvature processor 72, the second Kalman filter 54,and the second curvature processor 74, wherein the first curvatureprocessor 72 calculates the road curvature parameter C₀ and {dot over(C)} from the host state [ω, {dot over (ω)}, U, {dot over (U)}]^(T) forinput as a measurement vector Z_(k) ^(C) to the second Kalman filter 54,and the second curvature processor 74 transforms the output of thesecond Kalman filter 54 to C₀ and C₁={dot over (C)}/Û as the curvatureestimates of the curvature estimator 70. The associated second Kalmanfilter 54 is illustrated in FIG. 21, wherein the associated vectors andmatrices referenced in the Appendix are given by: $\begin{matrix}{{\underset{\_}{Z}}_{Ck} = {{\begin{bmatrix}C_{0m} \\{\overset{.}{C}}_{m}\end{bmatrix}_{k}\quad R_{Ck}} = \begin{bmatrix}\sigma_{C\quad 0}^{2} & \sigma_{C_{0}\overset{.}{C}}^{2} \\\sigma_{C_{0}\overset{.}{C}}^{2} & \sigma_{\overset{.}{C}}^{2}\end{bmatrix}}} & (118) \\{F_{k} = {{\begin{bmatrix}1 & T_{k} \\0 & 1\end{bmatrix}\quad Q_{k}} = {\begin{bmatrix}{T_{k}^{3}/3} & {T_{k}^{2}/2} \\{T_{k}^{2}/2} & T_{k}\end{bmatrix} \times q}}} & (119)\end{matrix}$wherein q=0.01, T_(k) is the sampling time interval, and the varianceand covariance elements of the associated R matrix are given byequations (97), (106) and (112).

A fifth embodiment of the curvature estimator 70 is a modification ofthe fourth embodiment, wherein the second Kalman filter 54 is adapted toincorporate a sliding window in the associated filtering process. Thelength of the sliding window is adapted so as to avoid excessive delaycaused by window processing, and for example, in one embodiment,comprises about 5 samples. The associated vectors andmatrices—referenced in the Appendix—of the associated second Kalmanfilter 54 are given by: $\begin{matrix}{{F_{k} = \begin{bmatrix}1 & {d\quad L} \\0 & 1\end{bmatrix}}{R_{Ck} = {\begin{bmatrix}{{mean}\text{(}{\sigma_{C\quad 0}^{2}\left( {k - {L\text{:}k}} \right)}\text{)}} & {{mean}\text{(}{\sigma_{C_{0}\overset{.}{C}}^{2}\left( {k - {L\text{:}k}} \right)}\text{)}} \\{{mean}\text{(}{\sigma_{C_{0}\overset{.}{C}}^{2}\left( {k - {L\text{:}k}} \right)}\text{)}} & {{mean}\text{(}{\sigma_{\overset{.}{C}}^{2}\left( {k - {L\text{:}k}} \right)}\text{)}}\end{bmatrix}/L}}} & (120) \\{{{\underset{\_}{Z}}_{Ck} = \begin{bmatrix}{{mean}\left( {C\quad 0_{m}\left( {k - {L\text{:}k}} \right)} \right)} \\{{mean}\text{(}{{\overset{.}{C}}_{m}\left( {k - {L\text{:}k}} \right)}\text{)}}\end{bmatrix}_{k}}{Q_{k} = {\begin{bmatrix}{\left( {d\quad L} \right)^{3}/3} & {\left( {d\quad L} \right)^{2}/2} \\{\left( {d\quad L} \right)^{2}/2} & {d\quad L}\end{bmatrix} \times q}}} & (121)\end{matrix}$wherein L is the sliding window length,${{d\quad L} = {\sum\limits_{i = {k - L}}^{k}T_{i}}},{{{and}\quad q} = {4 \times {10^{- 4}.}}}$The variances and covariance of the associated R matrix are given byequations (97), (106) and (112).

A sixth embodiment of the curvature estimator 70 is a modification ofthe fifth embodiment, wherein the length L of the sliding window isadaptive. For example, the window length can be adapted to be responsiveto the road curvature parameters C₀ and C₁, for example, in accordancewith the following rule of equation (117). This rule provides for alarger window length L—e.g. as large as 25 samples—when both C₀ and C₁are relatively small, for example, corresponding to a straight sectionof road. The window length L becomes smaller—e.g. as small as 1sample—when either C₀ or C₁ is large, corresponding to a turn ortransaction of road. Furthermore, the window length L can be sharplyreduced to account for a sudden vehicle maneuver, with a limitation onthe subsequent increase in window length L to one sample per step sothat the previous samples so not adversely affect the output from thecurvature estimator 70 when maneuver ends. As the window length L ischanged, the associated F, Q, and R matrices in the second Kalman filter54 are also changed.

In accordance with a seventh embodiment, the curvature estimator 70comprises the first curvature processor 72, which calculates the roadcurvature parameters C₀ and C₁ from the host state [ω, {dot over (ω)},U, {dot over (U)}]^(T) as the output of the road curvature estimationsubsystem 42—without using a second Kalman filter 54.

In accordance with an eighth embodiment, the curvature estimator 70comprises the first curvature processor 72, which determines the roadcurvature parameters C₀ and C₁ of the clothoid model by a curve fit of atrajectory of the host state [ω, {dot over (ω)}, U, {dot over (U)}]^(T)from the host state filter 52.1. The position, velocity and accelerationcomponents of the host vehicle 12 are calculated as follows:θ_(k)=θ_(k−1)+ω_(k−1) ·T+{dot over (ω)} _(k−1) ·T ²/2  (122)x _(k) =x _(k−1)+cos ∂_(k)·(U _(k−1) ·T+{dot over (U)} _(k−1) ·T²/2)  (123)y _(k) =y _(k−1)+sin θ_(k)·(U _(k−1) ·T+{dot over (U)} _(k−1) ·T²/2)  (124){dot over (x)} _(k) ={dot over (U)} _(k)·cos θ_(k)  (125){umlaut over (x)} _(k)·cos θ_(k) +U _(k)·ω_(k)·sin θ_(k)  (126){dot over (y)} _(k) =U _(k)·sin θ_(k)  (127)ÿ _(k) ={dot over (U)} _(k)·sin θ_(k) +U _(k)·ω_(k)·cos θ_(k)  (128)

Then from the clothoid model and equation (8): $\begin{matrix}{y_{k} = {{\left( \frac{x_{k}^{2}}{2} \right) \cdot C_{0}} + {\left( \frac{x_{k}^{3}}{6} \right) \cdot C_{1}}}} & (129) \\{{\overset{.}{y}}_{k} = {{\left( {x_{k} \cdot {\overset{.}{x}}_{k}} \right) \cdot C_{0}} + {\left( {x_{k}^{2} \cdot \frac{{\overset{.}{x}}_{k}}{2}} \right) \cdot C_{1}}}} & (130) \\{{\overset{¨}{y}}_{k} = {{\left( {{\overset{.}{x}}_{k}^{2} + {x_{k} \cdot {\overset{¨}{x}}_{k}}} \right) \cdot C_{0}} + {\left( {{x_{k} \cdot {\overset{.}{x}}_{k}^{2}} + {x_{k}^{2} \cdot \frac{{\overset{¨}{x}}_{k}}{2}}} \right) \cdot C_{1}}}} & (131)\end{matrix}$

After finding x_(k), y_(k) and their derivatives using equations (122)through (128), equations (129) through (131) are used with curve fittingto solve for the road curvature parameters C₀ and C₁, wherein a windowof sampled data points—e.g. in one embodiment, about 12 sample points—isused to improve the smoothness of the associated curve used for curvefitting.

A ninth embodiment of the curvature estimator 70 is a modification ofthe eighth embodiment, wherein the length L of the sliding window isadaptive. For example, the window length can be adapted to be responsiveto the road curvature parameters C₀ and C₁, for example, in accordancewith the following rule:L _(k)=min{max{25−floor[(|C0_(m)(k)|×|C1_(m)(k)|)×7000],2},L_(k−1)+1}  (132)This rule provides for a larger window length L—e.g. as large as 25samples—when both C₀ and C₁ are relatively small, for example,corresponding to a straight section of road. The window length L becomessmaller—e.g. as small as 2 samples—when either C₀ or C₁ is large,corresponding to a turn or transition of road. Furthermore, the windowlength L can be sharply reduced to account for a sudden vehiclemaneuver, with limitation on the subsequent increase in window length Lto one sample per step so that the previous samples so not adverselyaffect the output from the curvature estimator 70 when maneuver ends.

The above-described first through ninth embodiments of the roadcurvature estimation subsystem 42 are based upon the clothoid model ofroad curvature, wherein the road curvature C is characterized as varyinglinearly with respect to path length along the road, and whereindifferent types of roads (e.g. straight, circular or generally curved)are represented by different values of the clothoid road curvatureparameters C₀ and C₁. The clothoid model reduces to a simpler form forstraight (C₀=C₁=0) and circular (C₁=0) road segments. Differentembodiments of the road curvature estimation subsystem 42 can be usedunder different conditions. For example, in one embodiment of thepredictive collision sensing system 10 in which the sampling rate of theyaw rate sensor 16 is relatively low, the seventh embodiment of thecurvature estimator 70 is used when the longitudinal speed U of the hostvehicle 12 is greater than a threshold, e.g. about 11 meters/second, andthe ninth embodiment of the curvature estimator 70 is used at greatervelocities. The ratio of the mean prediction error to the meanprediction distance can be used to compare and evaluate the variousembodiments of the curvature estimator 70.

Generally, high-speed roads can be modeled as a collection of differenttypes of interconnected road segments, each of which is represented by adifferent road model. For example, FIG. 22 illustrates a straight firstroad segment 76 having a linear path—represented by C=0,—connected to acurved second road segment 78 having a cubic path—represented by theclothoid model C=C₀+C₁x,—connected to a third road segment 80 having aquadratic path—represented by C=C₀.

Referring to FIG. 23, in accordance with a tenth embodiment of the roadcurvature estimation subsystem 42, different types of roads arerepresented with different models, rather than using a singleall-inclusive clothoid model, thereby providing for improved estimationaccuracy for road segments that are characterized by models with fewerdegrees of freedom resulting from constraints on one or morecoefficients of the clothoid model, because of the fewer degrees offreedom. For example, for a straight road segment 76, the clothoid modelreduces to C=0->C₀=C₁=0 corresponding to a path equation of y=0; and fora quadratic road segment 80, the clothoid model reduces to C=C₀corresponding to a path equation of $y = {\frac{1}{2}C_{0}{x^{2}.}}$Accordingly, the roadway is represented by a multiple model system 82,wherein a particular road segment is characterized by one of a finitenumber r of models. For example, in FIG. 23, the multiple model system82 incorporates r=3 models, each of which is embodied in a separatecorresponding curvature estimator 70.1, 70.2 and 70.3 respectively. Eachcurvature estimator 70.1, 70.2, 70.3 is generally structured asillustrated in FIG. 19, and is adapted to process the output of the hoststate filter 52.1. Although FIG. 23 illustrates a common host statefilter 52.1 for all of the curvature estimators 70.1, 70.2, 70.3,different curvature estimators 70.1, 70.2, 70.3 could utilize differentcorresponding embodiments of the host state filter 52.1—e.g. oneembodiment of the host state filter 52.1 using only a yaw rate sensor16, another embodiment using a yaw rate sensor 16 and a steer anglesensor 58. For example, the first curvature estimator 70.1 embodies astraight road model, the second curvature estimator 70.2 embodies acircular arc or quadratic road model and the third curvature estimator70.3 embodies a clothoid road model suitable for a general high-speedroad.

More particularly, the straight road model is characterized by:C=0=>(C₀=0 and C₁=0)  (133) $\begin{matrix}{F_{k} = {{\begin{bmatrix}0 & 0 \\0 & 0\end{bmatrix}\quad Q_{k}} = \begin{bmatrix}0 & 0 \\0 & 0\end{bmatrix}}} & (134)\end{matrix}$

The circular arc or quadratic road model is characterized by:C=C₀=>(C₁=0)  (135) $\begin{matrix}{F_{k} = {{\begin{bmatrix}1 & 0 \\0 & 0\end{bmatrix}\quad Q_{k}} = {\begin{bmatrix}{\text{(}{\hat{U}}_{k}T_{k}{\text{)}^{3}/3}} & 0 \\0 & 0\end{bmatrix} \times q}}} & (136)\end{matrix}$where T_(k) is sampling period, Û_(k) is estimated host speed, andq=0.0005.

The clothoid road model is characterized by:C=C ₀ +C ₁ ·l  (137) $\begin{matrix}{F_{k} = {{\begin{bmatrix}1 & {{\hat{U}}_{k}T_{k}} \\0 & 1\end{bmatrix}\quad Q_{k}} = {\begin{bmatrix}{\text{(}{\hat{U}}_{k}T_{k}{\text{)}^{3}/3}} & {\text{(}{\hat{U}}_{k}T_{k}{\text{)}^{2}/2}} \\{\text{(}{\hat{U}}_{k}T_{k}{\text{)}^{2}/2}} & {{\hat{U}}_{k}T_{k}}\end{bmatrix} \times q}}} & (138)\end{matrix}$where T_(k) is sampling period, Û_(k) is estimated host speed, andq=0.0025.

The multiple model system 82 has both continuous (noise) uncertaintiesas well as discrete (“model” or “mode”) uncertainties, and is therebyreferred to as a hybrid system. The multiple model system 82 is assumedto be characterized by a base state model, a modal state, and a modejump process.

The base state model is assumed to be characterized as follows:x(k)=F[M(k)]x(k−1)+v[k−1, M(k)]  (139)z(k)=H[M(k)]x(k)+w[k, M(k)]  (140)where M(k) denotes the mode at time k in effect during the samplingperiod ending at k.

The modal state, or mode, is assumed to be one of r possible modes:M(k)∈{M_(j)}^(r) _(j=1)  (141)wherein the structure of the system and/or the statistics of theassociated noise components can be different for different modes, asfollows:F[M_(j)]=F_(j)  (142)v(k−1, M_(j))˜N(u_(j), Q_(j))  (143)

The mode jump process governs the transition from one mode to another isassumed to be characterized by a Markov chain with known transitionprobabilities, as follows:P{M(k)=M _(j) |M(k−1)=M_(i) }=p _(ij)  (144)

The curvature estimators 70.1, 70.2, 70.3 operate in parallel, and theoutput therefrom is operatively coupled to a curvature processor84—which, for example, can be embodied in the signal processor 26—whichgenerates a single estimate of road curvature and associated covariance,in accordance with an interacting multiple model algorithm 2400 (IMM).Generally, the interacting multiple model algorithm 2400 is useful totrack either or both maneuvering and non-maneuvering targets withmoderate computational complexity, wherein a maneuver is modeled as aswitching of the target state model governed by an underlying Markovchain. Different state models can have different structures, and thestatistics of the associated process noises of different state modelscan be different. The interacting multiple model algorithm 2400 performssimilar to the exact Bayesian filter, but requires substantially lesscomputational power. Each model has a corresponding filter that isprocessed by the curvature processor 84 in accordance with theinteracting multiple model algorithm 2400.

Referring to FIG. 24, the interacting multiple model algorithm 2400commences with the initialization in the current cycle of each modeconditioned filter in step (2402), whereby the mode-conditioned stateestimates and covariance of the previous cycle are mixed using mixingprobabilities. Each filter uses a mixed estimate at the beginning ofeach cycle, wherein the mixing is determined by the probabilities ofswitching between models. The initial estimate and covariance for filterj, for each model, i.e. j=1, . . . , r, is given by: $\begin{matrix}{{{\hat{x}}^{0j}\left( {{k - 1}❘{k - 1}} \right)} = {\sum\limits_{i = 1}^{r}{{{\hat{x}}^{i}\left( {{k - 1}❘{k - 1}} \right)} \cdot {\mu_{i❘j}\left( {{k - 1}❘{k - 1}} \right)}}}} & (145) \\{{p^{0j}\left( {{k - 1}❘{k - 1}} \right)} = {\sum\limits_{i = 1}^{r}{{\mu_{i❘j}\left( {{k - 1}❘{k - 1}} \right)} \cdot \left\{ {{P^{i}\left( {{k - 1}❘{k - 1}} \right)} + {\left\lbrack {{{\hat{x}}^{i}\left( {{k - 1}❘{k - 1}} \right)} - {{\hat{x}}^{0j}\left( {{k - 1}❘{k - 1}} \right)}} \right\rbrack \cdot \left\lbrack {{{\hat{x}}^{i}\left( {{k - 1}❘{k - 1}} \right)} - {{\hat{x}}^{0j}\left( {{k - 1}❘{k - 1}} \right)}} \right\rbrack^{\prime}}} \right\}}}} & (146)\end{matrix}$

Then, in step (2404), the mode-conditioned state is propagated for eachmodel, i.e. j=1, . . . , r, according to a Kalman filter matched to thej^(th) mode, M_(j)(k), so as to provide the state x^(j)(k|k−1) andcovariance P^(j)(k|k−1) at time k.

Then, in step (2406), the propagated mode-conditioned state estimatesand covariances for each of the modes are combined to give:$\begin{matrix}{{\hat{x}\left( {k❘{k - 1}} \right)} = {\sum\limits_{j = 1}^{r}{{{\hat{x}}^{j}\left( {k❘{k - 1}} \right)}{\mu_{j}\left( {k - 1} \right)}}}} & (147) \\{{P\left( {k❘{k - 1}} \right)} = {\sum\limits_{j = 1}^{r}{{\mu_{j}\left( {k - 1} \right)}\left\{ {{P^{j}\left( {k❘{k - 1}} \right)} + {\left\lbrack {{{\hat{x}}^{j}\left( {k❘{k - 1}} \right)} - {\hat{x}\left( {k❘{k - 1}} \right)}} \right\rbrack\left\lbrack {{{\hat{x}}^{j}\left( {k❘{k - 1}} \right)} - {\hat{x}\left( {k❘{k - 1}} \right)}} \right\rbrack}^{\prime}} \right\}}}} & (148)\end{matrix}$

Then, in step (2408), for each of the r parallel filters, the stateestimates and covariances are calculated, conditioned on a mode being ineffect and conditioned on the corresponding mode likelihood function.The Kalman filter matched to the j^(th) mode, M_(j)(k), uses measurementz(k) to provide the state x^(j)(k|k) and covariance P^(j)(k|k), wherebythe likelihood function corresponding to filter j is given by:Λ_(j)(k)=N[z(k); z _(j)(k|k−1), S _(j)(k)]  (149)

Then, in step (2410), the model probability μ_(j)(k) is updated for eachmode, i.e. j=1, . . . , r, in parallel so as to provide the likelihoodthat each model is correct. The mixing probabilities are calculated forall combinations of (i,j) for (i,j=1, . . . r), as follows:$\begin{matrix}{{{\mu_{i❘j}\left( {{k - 1}❘{k - 1}} \right)} = {\frac{1}{{\overset{\_}{c}}_{j}}p_{ij}{\mu_{i}\left( {k - 1} \right)}}}{where}} & (150) \\{{\overset{\_}{c}}_{j} = {\sum\limits_{i = 1}^{r}{p_{ij}{\mu_{i}\left( {k - 1} \right)}}}} & (151)\end{matrix}$The mode probabilities are then updated for each mode, i.e. j=1, . . . ,r, as follows: $\begin{matrix}{{\mu_{j}(k)} = {\frac{1}{c}{\Lambda_{j}(k)}{\overset{\_}{c}}_{j}}} & (152) \\{c = {\sum\limits_{j = 1}^{r}{{\Lambda_{j}(k)}{\overset{\_}{c}}_{j}}}} & (153)\end{matrix}$

Then, in step (2412), the overall state estimate and covariance—theoutput of the interacting multiple model algorithm 2400—is calculated bycombining the mode-conditioned state estimates and covariances asfollows: $\begin{matrix}{{\hat{x}\left( {k❘k} \right)} = {\sum\limits_{j = 1}^{r}{{{\hat{x}}^{j}\left( {k❘k} \right)}{\mu_{j}(k)}}}} & (154) \\{{P\left( {k❘k} \right)} = {\sum\limits_{j = 1}^{r}{{\mu_{j}(k)}\left\{ {{P^{j}\left( {k❘k} \right)} + {\left\lbrack {{{\hat{x}}^{j}\left( {k❘k} \right)} - {\hat{x}\left( {k❘k} \right)}} \right\rbrack\left\lbrack {{{\hat{x}}^{j}\left( {k❘k} \right)} - {\hat{x}\left( {k❘k} \right)}} \right\rbrack}^{\prime}} \right\}}}} & (155)\end{matrix}$

The measurement and its noise covariance used as input to theinteracting multiple model algorithm 2400 curvature filter, for each ofthe three associated mode-conditioned Kalman filters, are given by:$\begin{matrix}{{\begin{bmatrix}C_{0m} \\C_{1m}\end{bmatrix}_{k} = \begin{bmatrix}\frac{{\hat{\omega}}_{k}}{{\hat{U}}_{k}} \\{\frac{{\hat{\overset{.}{\omega}}}_{k}}{{\hat{U}}_{k}^{2}} - \frac{{\hat{\omega}}_{k}{\hat{\overset{.}{U}}}_{k}}{{\hat{U}}_{k}^{3}}}\end{bmatrix}},} & (156)\end{matrix}$R_(Ck)=J_(CX)P_(k|k)J_(CX) ^(T)  (157)where P_(k|k) is the state error covariance matrix from the host filter,and J_(CX) is the Jacobian matrix given by: $\begin{matrix}{J_{CX} = \begin{bmatrix}{- \frac{{\hat{\omega}}_{k}}{{\hat{U}}_{k}^{2}}} & 0 & \frac{1}{{\hat{U}}_{k}} & 0 \\{{- \frac{2{\hat{\overset{.}{\omega}}}_{k}}{{\hat{U}}_{k}^{3}}} + \frac{3{\hat{\omega}}_{k}{\hat{\overset{.}{U}}}_{k}}{{\hat{U}}_{k}^{4}}} & {- \frac{{\hat{\omega}}_{k}}{{\hat{U}}_{k}^{3}}} & {- \frac{{\hat{\overset{.}{U}}}_{k}}{{\hat{U}}_{k}^{3}}} & \frac{1}{{\hat{U}}_{k}^{2}}\end{bmatrix}} & (158)\end{matrix}$

The Markov model is implemented by assuming that at each scan time thereis a probability p_(ij) that the target will make the transition frommodel state i to state j. These probabilities are assumed to be known apriori and can be expressed in a model probability transition matrix,e.g. as follows: $\begin{matrix}{\quad{{{New}\quad{State}}\quad\begin{matrix}1 & {\quad 2\quad} & 3\end{matrix}\quad\begin{matrix}{P_{trans} = {\begin{matrix}{Prior} \\{State}\end{matrix}\begin{matrix}1 \\2 \\3\end{matrix}\begin{matrix}\left\lbrack \begin{matrix}p_{11} \\p_{21} \\p_{31}\end{matrix} \right. & \begin{matrix}p_{12} \\p_{22} \\p_{32}\end{matrix} & \left. \begin{matrix}p_{13} \\p_{23} \\p_{33}\end{matrix} \right\rbrack\end{matrix}}} \\{= \begin{bmatrix}0.95 & 0.01 & 0.04 \\0.07 & 0.75 & 0.18 \\0.17 & 0.16 & 0.67\end{bmatrix}}\end{matrix}}} & (159)\end{matrix}$

Referring to FIGS. 1 and 25, in accordance with another embodiment of aroad curvature estimation subsystem 42′, the predictive collisionsensing system 10 further comprises a vehicle navigation system 86 andan associated map system 88, operatively coupled to the signal processor26. For example, the vehicle navigation system 86 could comprise a GPSnavigation system that provides at least a two-dimensional vehicleposition measure Z _(k) ^(g) indicative of the current vehicle positionon the road surface, e.g. Z _(k) ^(g)=[x, y], having an associated errorcovariance R ^(g)=cov (Z _(k) ^(g)), where x and y are the worldabsolute coordinates of the vehicle, e.g. latitude and longitudecoordinates in the World Geodetic System WGS. The vehicle navigationsystem 86 could also provide altitude and time information, and/or theassociated velocities of the measurements. The vehicle position measureZ _(k) ^(g) is used with the map system 88 to determine the position andcurvature coordinates of the road relative to the host vehicle 12,wherein the map system 88 incorporates a digital map database inabsolute coordinates and an algorithm to convert the vehicle positionmeasure Z _(k) ^(g)—in world absolute coordinates—of the road on whichthe host vehicle 12 is located, to the host absolute coordinates in thecoordinate system used hereinabove for road curvature and target stateestimation. Accordingly, the map system 88 provides for the followingtransformation:[ X ^(c) , Y ^(c) , C ^(R) ]=f ^(M)(x, y)  (160)wherein (x, y) are the world absolute coordinates of the vehicleposition measure Z _(k) ^(g) from the vehicle navigation system 86; X^(c) and Y ^(c) are vectors containing the coordinates of the center ofthe road closest to [x, y], and C ^(R) is an array containing thecurvature parameters corresponding to the road center point coordinatesin the vectors X ^(c) and Y ^(c). Accordingly, [X ^(c)(i), Y ^(c)(i)]represents a point on the center of the road, and C ^(R)(i)=[C⁰, C¹]represents the curvature parameters of the road at that point. The errorcovariance of X ^(c) and Y ^(c) is R _(m) ^(R), and the error covarianceof C ^(R) is given by R _(C) ^(g), representing the noise or error inthe associated map data. The map system 88 can either store both theroad position coordinates and associated curvature parameterinformation, or could calculate the curvature parameters from the storedposition coordinates of the road centers as the information is required.The world absolute coordinates can be transformed to host vehicleabsolute coordinates by a combination of translation and rotation, giventhe position of the host vehicle 12 from the vehicle navigation system86, and the heading of the host vehicle 12 based upon either informationfrom the host filter 52.1, or from the vehicle navigation system 86, orboth.

More particularly, given the vehicle position measure Z_(k) ^(g), in oneembodiment the map system 88 uses an associated map database todetermine the road that the host vehicle 12 is most likely on by findingroads in the associated map database that satisfy the followingselection criteria:( Z _(k) ^(g) −Z ^(r))· R ^(g)·( Z _(k) ^(g) −Z ^(r))≦T  (161)where T is a selection threshold and Z ^(r) is a point on the road thatis closest to the vehicle position measure Z _(k) ^(g). If more than oneroad satisfies the selection criteria, then the most likely road isselected by comparing the curvature${\hat{\underset{\_}{C}}}_{k}^{C} = \begin{bmatrix}\hat{C_{0}} \\\hat{C_{1}}\end{bmatrix}_{k❘k}^{C}$estimated by another road curvature estimation system 42 with thecurvature C ^(R) of each prospective road from the map database of themap system 88, and the road from the map database, for which thecurvature C ^(R) of the point nearest to the vehicle position measure Z_(k) ^(g) is closest to estimated curvature Ĉ _(k) ^(C) from the othercurvature estimation system 42, is selected as the most likely road fromthe map database, and the associated curvature at the closest point isthen given as C _(5|k) ^(g)=C ^(R).

Referring to FIG. 25, the curvature C _(k|k) ^(g) from the map system 88of a point on the road closest to the location of the host vehicle 12from the vehicle navigation system 86 is used as a measurement to anassociated Kalman filter 54′ of a corresponding curvature filter 54.1′,which is used to generate a corresponding map-based road curvatureestimate $\begin{bmatrix}\hat{C_{0}} \\\hat{C_{1}}\end{bmatrix}_{k❘k}^{C\_ g}$and an associated covariance P_(k|k) ^(C) ^(—) ^(g).

Referring to FIG. 26, in accordance with another embodiment of a roadcurvature estimation subsystem 42″, the associated trajectory$\begin{bmatrix}r \\\overset{.}{r} \\\eta\end{bmatrix}_{k}^{t}$of the target vehicle 36—as measured by the radar sensor 14—can be usedto determine an estimate of road curvature of the roadway 34 based uponthe premise that under normal driving conditions, the target vehicle 36is assumed to follow the roadway 34. The dynamics of the t^(th) targetare assumed to be given by the following constant-acceleration kinematicequations, which are embodied in an extended Kalman filter 90 of anauxiliary filter 90.1.x _(k+1) ^(at) =F _(k) ^(ay) ·x _(k) ^(at)+w _(k) ^(at) , w _(k) ^(at)˜N(0,Q _(k) ^(at))  (162)z _(k) ^(at) =h _(k) ^(at)(x _(k) ^(at))+v _(k) ^(at) , v _(k) ^(at)˜N(0,R _(k) ^(at))  (163)where $\begin{matrix}{{{F_{k}^{at} = \begin{bmatrix}F_{1} & 0 \\0 & F_{1}\end{bmatrix}},{F_{1} = \begin{bmatrix}1 & T & \frac{T^{2}}{2} \\0 & 1 & T \\0 & 0 & 1\end{bmatrix}},{{\underset{\_}{x}}_{k}^{at} = \begin{bmatrix}x^{at} \\{\overset{.}{x}}^{at} \\{\overset{¨}{x}}^{at} \\y^{at} \\{\overset{.}{y}}^{at} \\{\overset{¨}{y}}^{at}\end{bmatrix}_{k}}}{{{and}\quad{\underset{\_}{z}}_{k}^{at}} = \begin{bmatrix}r \\\overset{.}{r} \\\eta\end{bmatrix}_{k}^{t}}} & (164)\end{matrix}$wherein T is the sampling period, the superscript (•)^(t) is used todesignate a particular target, and the superscript (•)^(a) is used toindicate that the filter is auxiliary.

The associated measurement function h _(k) ^(at) is non-linear, and given by: $\begin{matrix}{{\underset{\_}{h}}_{k}^{at} = \begin{bmatrix}\sqrt{\left( x_{k}^{at} \right)^{2} + \left( y_{k}^{at} \right)^{2}} \\{\frac{1}{\sqrt{\left( x_{k}^{at} \right)^{2} + \left( y_{k}^{at} \right)^{2}}} \cdot \left( {{x_{k}^{at} \cdot {\overset{.}{x}}_{k}^{at}} + {y_{k}^{at} \cdot {\overset{.}{y}}_{k}^{at}}} \right)} \\{\tan^{- 1}\left( \frac{y_{k}^{at}}{x_{k}^{at}} \right)}\end{bmatrix}} & (165)\end{matrix}$

The extended Kalman filter 90 provides for linearization of thenon-linear measurement function h _(k) ^(at) using an associatedJacobian matrix, and provides for an estimate of the target state{circumflex over (x)} _(k|k) ^(at) and its associated error covarianceP_(k|k) ^(at) which are transformed by an associated measurementprocessor 92 so as to provide the measurement input z _(k) ^(C) ^(—)^(t) and associated error covariance R_(k) ^(C) ^(—) ^(t) to anassociated Kalman filter 94 of an associated curvature filter 94.1. Themeasurement input z _(k) ^(C) ^(—) ^(t) and associated error covarianceR_(k) ^(C) ^(—) ^(t) are given by: $\begin{matrix}{{\underset{\_}{z}}_{k}^{C\_ t} = \begin{bmatrix}{\hat{y}}^{at} \\{\hat{\overset{.}{y}}}^{at} \\{\hat{\overset{¨}{y}}}^{at}\end{bmatrix}_{k❘k}} & (166)\end{matrix}$R _(k) ^(C) ^(—) ^(t) =P _(y) _(k|k) ^(at)  (167)

The system equations of the associated Kalman filter 94 of theassociated curvature filter 94.1 are given by:x _(k+1) ^(C) ^(—) ^(t) =F _(k) ^(C) ^(—) ^(t) ·x _(k) ^(C) ^(—) ^(t) +w_(k) ^(C) ^(—) ^(t) , w _(k) ^(C) ^(—) ^(t) ˜N(0, Q _(k) ^(C) ^(—)^(t))  (168)z _(k) ^(C) ^(—) ^(t) =H ^(C) ^(—) ^(t) ·x _(k) ^(C) ^(—) ^(t) +v _(k)^(C) ^(—) ^(t) , v _(k) ^(C) ^(—) ^(t) ˜N(0, R _(k) ^(C) ^(—)^(t))  (169)where $\begin{matrix}{{F_{k}^{C\_ t} = \begin{bmatrix}1 & 0 & 0 \\0 & 1 & {{\Delta\quad{t \cdot \hat{U}}} + {{\hat{\overset{.}{U}} \cdot \Delta}\quad{t^{2}/2}}} \\0 & 0 & 1\end{bmatrix}},{{\underset{\_}{x}}_{k}^{C\_ t} = \begin{bmatrix}B \\C_{0} \\C_{1}\end{bmatrix}_{k}}} & (170)\end{matrix}$and the associated measurement matrix H_(k) ^(C) ^(—) ^(t) is given asfollows: $\begin{matrix}{H_{k}^{C\_ t} = \begin{bmatrix}1 & \frac{\left( {\hat{x}}^{at} \right)^{2}}{2} & \frac{\left( {\hat{x}}^{at} \right)^{3}}{6} \\0 & {{\hat{x}}^{at}{\hat{\overset{.}{x}}}^{at}} & \frac{\left( {\hat{x}}^{at} \right)^{2}{\hat{\overset{.}{x}}}^{at}}{2} \\0 & {{{\hat{x}}^{at}{\hat{\overset{¨}{x}}}^{at}} + \left( {\hat{\overset{.}{x}}}^{at} \right)^{2}} & {{{\hat{x}}^{at}\left( {\hat{\overset{.}{x}}}^{at} \right)}^{2} + \frac{\left( {\hat{x}}^{at} \right)^{2}{\hat{\overset{¨}{x}}}^{at}}{2}}\end{bmatrix}_{k❘k}} & (171)\end{matrix}$

The auxiliary filter 92.1 and the curvature filter 94.1 of the roadcurvature estimation subsystem 42″ operate in accordance with theAppendix.

Referring to FIG. 27, in accordance with another embodiment, apredictive collision sensing system 10.1 comprises a plurality of roadcurvature estimation subsystems 42.1, 42.2, . . . , 42.N, each of whichoperates in accordance with any one of the various embodiments of roadcurvature estimation subsystems 42, 42′ or 42″ described hereinabove,e.g. as illustrated in FIGS. 4, 19, 23, 25 or 26. Each road curvatureestimation subsystem 42.1, 42.2, . . . 42.N provides a separate,associated estimate of road curvature, which is then fused by a roadcurvature fusion subsystem 96, for example, to provide an improvedestimate of the road curvature and associated error covariance at afuture location along the roadway 34. For example, in one embodiment,the predictive collision sensing system 10.1 incorporates first 42.1 andsecond 42.2 road curvature estimation subsystems, wherein the first roadcurvature estimation subsystem 42.1 is a road curvature estimationsubsystem 42 responsive to host vehicle measurements, and the secondroad curvature estimation subsystem 42.2 is a road curvature estimationsubsystem 42′ responsive to measurements from a vehicle navigationsystem 86 and an associated map system 88.

From the first road curvature estimation subsystem 42.1, the curvatureestimate and associated error covariance at a distance l along theroadway 34 from the current location are given respectively by:C(l)=C ₀ +C ₁ ·l  (172)R _(C)(l)=R _(C0) +R _(C1) ·l  (173)where R_(C0), R_(C1) and R_(C) are the error covariances of C₀, C₁ and Crespectively.

From the second road curvature estimation subsystem 42.2, thecorresponding curvature estimate is given by C_(g)(l) and thecorresponding error covariance is given by R_(g), wherein R_(g) isgenerally a constant scalar.

The curvature estimates and associated error covariances are combined bythe road curvature fusion subsystem 96, for example, as follows:C _(f)(l)=G·C(l)+G _(g) ·C _(g)(l)  (174)R _(f)(l)=(R _(C)(l)⁻¹ +R _(g) ⁻¹)⁻¹  (175)where G and G_(g) are weights given by: $\begin{matrix}{G = \frac{1}{1 + \frac{R_{C}(l)}{R_{g}}}} & (176) \\{G_{g} = \frac{1}{1 + \frac{R_{g}}{R_{C}(l)}}} & (177)\end{matrix}$

The fused curvature C_(f)(l) and associated error covariance R_(f)(l)can be used by other processes, for example for improving the estimationof the locations of the host 12 or target 36 vehicles, or for collisionprediction. For example, in the embodiment of FIG. 27, the fusedcurvature C_(f)(l) and associated error covariance R_(f)(l) are input toa constrained target state estimation subsystem 46 for estimating theconstrained target state, wherein the constrained target stateestimation subsystem 46 and the associated unconstrained target stateestimation subsystem 44, target state decision subsystem 48, and targetstate fusion subsystem 50 function in accordance with the embodimentillustrated in FIG. 4.

While specific embodiments have been described in detail in theforegoing detailed description and illustrated in the accompanyingdrawings, those with ordinary skill in the art will appreciate thatvarious modifications and alternatives to those details could bedeveloped in light of the overall teachings of the disclosure.Accordingly, the particular arrangements disclosed are meant to beillustrative only and not limiting as to the scope of the invention,which is to be given the full breadth of the any claims derivable fromthe description herein, and any and all equivalents thereof.

APPENDIX—DESCRIPTION OF KALMAN FILTERING

A Kalman filter is used to estimate, from a set of noisy measurements,the state and associated covariance of a dynamic system subject tonoise.

The system dynamics are defined by:x _(k+1) +F _(k) ·x _(k) +w _(k), w _(k) ˜N(0, Q _(k))  (A-1)where x _(k) is the system state vector, F_(k) is the system matrix andw _(k) an associated vector of noise variables corresponding to eachstate variable, each noise variable having a mean value of zero, and avariance given by the corresponding element of the associated variancevector, Q_(k).

The dynamics of the associated system measurements are given by:z _(k) =H _(k) ·x _(k) +v _(k), v _(k) ˜N(0, R _(k))  (A-2)where z _(k) is the system measurement vector, H_(k) is the measurementmatrix and v _(k) an associated vector of noise variables correspondingto each measurement variable, each noise variable having a mean value ofzero, and a variance given by the corresponding element of theassociated variance vector, R_(k). The values of the elements of theassociated covariance matrix R_(k) can be determined a priori fromanalysis of the representative measurements of the associated system forassociated representative sets of operating conditions. The values ofthe elements of the associated covariance matrix Q_(k) account formodeling errors. Generally, the associated matrices F_(k), Q_(k), H_(k),R_(k) can vary over time.

Given a measurement z _(k) at time k, and initial values of the statexx_(k−1|k−1) and associated covariance P_(k−1|k−1) at time k−1, theKalman filter is used to estimate the associated state x _(k|k) andassociated covariance P_(k|k) at time k.

The first step in the filtering process is to calculate estimates of thestate xk|k−1 and associated covariance P_(k|k−1) at time k based uponestimates at time k-1, as follows:x _(k|k−1) =F _(k) · _(k−1|k−1)  (A-3)P _(l|k−1) =F _(k) ·P _(k−1|k−1) ·F _(k) ^(T) +Q _(k)  (A-4)

The next step is to predict the measurement {circumflex over (z)}_(k)and associated covariance matrix S_(k) at time k, as follows:{circumflex over (z)}_(k) =H _(k) ·x _(k|k−1)  (A-5)S _(k) =cov({circumflex over (z)} _(k))=H _(k) ·P _(k|k−1) ·H _(k) ^(T)+R _(k)  (A-6)

The next step is to calculate a gain matrix G_(k) used for updating thestate vector x _(k|k) and associated covariance matrix P_(k|k), asfollows:G _(k) =P _(k|k−1) ·H _(k) ^(T) ·S _(k) ⁻¹  (A-7)

Finally, the state vector x _(k|k) and associated covariance matrixP_(k|k) are estimated at time k, responsive to the associatedmeasurement z _(k), as follows:x _(k|k) =x _(k|k−1) +G _(k)·( z _(k) −{circumflex over (z)}_(k))  (A-8)P _(k|k) =P _(k|k−1) −G _(k) ·S _(k) ·G _(k) ^(T)  (A-9)

1. A road curvature estimation system, comprising: a. a speed sensoradapted to measure a longitudinal speed of a vehicle on a roadway and togenerate a measure of longitudinal speed responsive thereto; b. a yawrate sensor adapted to measure a yaw rate of said vehicle and togenerate a first measure of yaw rate responsive thereto; c. a steerangle sensor adapted to measure a steer angle associated with at leastone steered wheel of said vehicle and to generate a first measure ofsteer angle responsive thereto; and d. a processor operatively coupledto said speed sensor, to said angular rate sensor and to said steerangle sensor, wherein if said longitudinal speed of said vehicle isgreater than a threshold, said processor is adapted to estimate acurvature of said roadway responsive to said first measure of yaw rateand to said measure of longitudinal speed, and if said longitudinalspeed of said vehicle is less than a threshold, said processor isadapted to generate and estimate of at least one curvature parameter ofsaid roadway responsive to said measure of steer angle and to saidmeasure of longitudinal speed, regardless of the curvature of saidroadway.
 2. A road curvature estimation system as recited in claim 1,wherein said processor comprises at least one Kalman filter, and said atleast one Kalman filter is adapted to generate an estimate of at leastone curvature parameter responsive to said measure of longitudinal speedand at least one of said first measure of yaw rate and a second measureof yaw rate responsive to said measure of steer angle and said measureof longitudinal speed, wherein said at least one curvature parameter isrepresentative of a curvature of said roadway.
 3. A road curvatureestimation system as recited in claim 1, wherein said at least oneKalman filter comprises first and second Kalman filters, said firstKalman filter is adapted generate an output comprising an estimate of ahost state responsive to said measure of longitudinal speed and to saidfirst or said second measure of said yaw rate, said host state comprisesa yaw rate, a yaw acceleration, a longitudinal speed and a longitudinalacceleration and said second Kalman filter is adapted to generate saidestimate of said at least one curvature parameter responsive to saidoutput from said first Kalman filter.
 4. A road curvature estimationsystem as recited in claim 1, wherein said processor comprises at leastone Kalman filter, at least one said Kalman filter is adapted generatean output comprising an estimate of a trajectory of said vehicleresponsive to said measure of longitudinal speed and to said first orsaid second measure of said yaw rate, and said processor is adapted todetermine said at least one said estimate of said at least one curvatureparameter from a curve fit of said host trajectory.
 5. A road curvatureestimation system as recited in claim 4, wherein said curve fit spans asliding window.
 6. A road curvature estimation system as recited inclaim 5, wherein a length of said sliding window is adapted responsiveto at least one said estimate of said at least one curvature parameter.7. A road curvature estimation system, comprising: a. a speed sensoradapted to measure a longitudinal speed of a vehicle on a roadway and togenerate a measure of longitudinal speed responsive thereto; b. a sourceof a measure of yaw rate of said vehicle; and c. a processor operativelycoupled to said speed sensor and to said source of said measure of yawrate, wherein said processor comprises at least one Kalman filter, andsaid at least one Kalman filter is adapted to generate an estimateassociated with at least one curvature parameter responsive to saidmeasure of longitudinal speed and to said measure of yaw rate, whereinsaid at least one curvature parameter is representative of a curvatureof said roadway and at least one said Kalman filter comprises a slidingwindow.
 8. A road curvature estimation system as recited in claim 7,wherein a length of said sliding window is adapted responsive to atleast one said estimate of said at least one curvature parameter.
 9. Aroad curvature estimation system, comprising: a. a speed sensor adaptedto measure a longitudinal speed of a vehicle on a roadway and togenerate a measure of longitudinal speed responsive thereto; b. a sourceof a measure of yaw rate of said vehicle; and c. a processor operativelycoupled to said speed sensor and to said source of said measure of yawrate, wherein said processor comprises first and second Kalman filters,said first Kalman filter is adapted generate an output comprisingestimates of a plurality of host state variables responsive to saidmeasure of longitudinal speed and said measure of said yaw rate, saidplurality of host state variables comprise yaw rate, yaw acceleration,longitudinal speed and longitudinal acceleration; said second Kalmanfilter is adapted to generate an estimate of at least one state variableresponsive to a measurement calculated from a plurality of said hoststate variables, and said at least one curvature parameter is calculatedfrom at least one state variable of said second Kalman filter.
 10. Aroad curvature estimation system as recited in claim 9, wherein at leastone said state variable of said second Kalman filter is related to atime derivative of curvature.
 11. A road curvature estimation system,comprising: a. a speed sensor adapted to measure a longitudinal speed ofa vehicle on a roadway and to generate a measure of longitudinal speedresponsive thereto; b. a source of a measure of yaw rate of saidvehicle; and c. a processor operatively coupled to said speed sensor andto said source of said measure of yaw rate, wherein said processorcomprises a plurality of curvature estimators associated with acorresponding plurality of different roadway models, each said curvatureestimator is adapted to estimate at least one associated curvatureparameter of a corresponding said different roadway model, saidprocessor provides for selecting a most likely of said different roadwaymodels as a most likely roadway model, and said processor provides foroutputting said at least one curvature parameter of said most likelyroadway model as an estimate of curvature of said roadway.
 12. A roadcurvature estimation system as recited in claim 11, wherein saidprocessor comprises an interacting multiple model algorithmincorporating a mode jump process characterized by a Markov chain.
 13. Aroad curvature estimation system as recited in claim 12, wherein saidinteracting multiple model algorithm comprises: a. a mode-conditionedfilter initialization process; b. a mode-conditioned state propagationprocess; c. a combination of mode-conditioned state and covariancepropagations; d. a mode-conditioned update process; e. a probabilityevaluation and update process; and f. process to output state andcovariance estimates.
 14. A road curvature estimation system as recitedin claim 11, wherein said plurality of different roadway models compriseat least two roadway models selected form a straight road model, aquadratic road model, and a clothoid road model.
 15. A road curvatureestimation system, comprising: a. a plurality of road curvatureestimation subsystems selected from a first road curvature estimationsubsystem adapted to estimate a first set of at least one firstcurvature parameter responsive to a measure of longitudinal speed of ahost vehicle on a rodway and a measure of yaw rate of said host vehicle,a second road curvature estimation subsystem adapted to estimate asecond set of at least one second curvature parameter responsive to ameasure of said second set of at least one second curvature parameterfrom a map database responsive to a measure of location of said hostvehicle, and a third road curvature estimation subsystem adapted toestimate a third set of at least one third curvature parameterresponsive to a radar measurement of a target vehicle traveling on saidroadway; and b. a processor adapted to fuse at least two of said firstset of at least one first curvature parameter, said second set of atleast one second curvature parameter, and said third set of at least onethird curvature parameter so as to generate a fourth set of at least onefourth curvature parameter as an estimate of curvature of said roadway.16. A road curvature estimation system as recited in claim 15, whereinsaid second road curvature estimation subsystem comprises: a. a vehiclenavigation system adapted to provide said measure of location of saidhost vehicle; b. said map database adapted to provide said measure ofsaid second set of at least one second curvature parameter responsive tosaid measure of location; and c. a Kalman filter adapted to estimatesaid second set of at least one road curvature parameter.
 17. A roadcurvature estimation system as recited in claim 15, wherein said thirdroad curvature estimation subsystem comprises: a. a radar sensor adaptedto provide a measure of a trajectory of said target vehicle; b. anextended Kalman filter adapted to provide a target state vectorresponsive to said measure of said trajectory of said target vehicle;and c. a curvature filter adapted to generate said estimate of saidthird set of at least one third curvature parameter responsive to ameasure of said third set of at least one third curvature parameterresponsive to said target state vector.
 18. A road curvature estimationsystem as recited in claim 15, wherein said processor generates said atleast one fourth curvature parameter from a weight combination of atleast two of said first set of at least one first curvature parameter,said second set of at least one second curvature parameter, and saidthird set of at least one third curvature parameter.
 19. A roadcurvature estimation system as recited in claim 15, wherein saidprocessor generates and error covariance associated with said at leastone fourth curvature parameter from a weight combination of at least twoof a first error covariance associated with said first set of at leastone first curvature parameter, a second error covariance associated withsaid second set of at least one second curvature parameter, and a thirderror covariance associated with said third set of at least one thirdcurvature parameter.